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Robot Manipulators and Control Systems 


2.1 Introduction 


This book focuses on industrial robotic manipulators and on industrial 
manufacturing cells built using that type of robots. This chapter covers the current 
practical methodologies for kinematics and dynamics modeling and computations. 
The kinematics model represents the motion of the robot without considering the 
forces that cause the motion. The dynamics model establishes the relationships 
between the motion and the forces involved, taking into account the masses and 
moments of inertia, i.e., the dynamics model considers the masses and inertias 
involved and relates the forces with the observed motion, or instead calculates the 
forces necessary to produce the required motion. These topics are considered very 
important to study and efficient use of industrial robots. 


Both the kinematics and dynamics models are used currently to design, simulate, 
and control industrial robots. The kinematics model is a prerequisite for the 
dynamics model and fundamental for practical aspects like motion planning, 
singularity and workspace analysis, and manufacturing cell graphical simulation. 
For example, the majority of the robot manufacturers and many independent 
software vendors offer graphical environments where users, namely developers and 
system integrators, can design and simulate their own manufacturing cell projects 
(Figure 2.1). 


Kinematics and dynamics modeling is the subject of numerous publications and 
textbooks [1-4]. The objective here is to present the topics without prerequisites, 
covering the fundamentals. Consequently, a real industrial robot will be used as an 
example which makes the chapter more practical, and easier to read. Nevertheless, 
the reader is invited to seek further explanation in the following very good sources: 


1. Introduction to Robotics, JJ Craig, John Willey and Sons, Chapters 2 to 7. 
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2. Modeling and Control of Robotic Manipulators, F. Sciavicco and B. 
Siciliano, Mcgraw Hill, Chapters 2 to 5. 

3. Handbook of Industrial Robotics, 2 edition, Shimon Nof, Chapter 6 
written by A. Goldenberg and M. Emani. 


D 


Figure 2.1 Aspect of a graphical simulation package (RobotStudio — ABB Robotics) 


Another important practical aspect is the way how these topics are implemented 
and used by actual robot control systems. This chapter also reviews the 
fundamental aspects of robot control systems from the perspective of an engineer 
and of a system integrator. The objective is to introduce the main components and 
modules of modern robot control systems, by examining some of the control 
systems available commercially. 


2.2 Kinematics 


Actual industrial robot manipulators are very advanced machines exhibiting high 
precision and repeatability. It's common to have medium payload robots (16 to 
20kg of payload) offering repeatability up to 0.1 mm, with smaller robots 
exhibiting even better performances (up to 0.01 mm). These industrial robots are 
basically composed by rigid links, connected in series by joints (normally six 
joints), having one end fixed (base) and another free to move and perform useful 
work when properly tooled (end-effector). As with the human arm, robot 
manipulators use the first three joints (arm) to position the structure and the 
remaining joints (wrist, composed of three joints in the case of the industrial 
manipulators) are used to orient the end-effector. There are five types of arms 
commonly used by actual industrial robot manipulators (Figure 2.2): cartesian, 
cylindrical, polar, SCARA and revolution. 
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SCARA Revolution 
Figure 2.2 Types of arms used with actual robot manipulators 
In terms of wrist designs, there are two main configurations (Figure 2.3): 


1. pitch-yaw-roll (XYZ) like the human arm 
2. roll-pitch-roll (ZYZ) or spherical wrist 


roli-pitch-roll (ZYZ) or spherical Wrist pitch-yaw-roll (YXZ) 
Figure 2.3 Wrist design configurations 


The spherical wrist is the most popular because it is mechanically simpler to 
implement. Nevertheless, it exhibits singular configurations that can be identified 
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and consequently avoided when operating with the robot. The trade between 
simplicity of robust solutions and the existence of singular configurations is 
favorable to the spherical wrist design, and that is the reason for its success. 


The position and orientation of the robot's end-effector (tool) is not directly 
measured but instead computed using the individual joint position readings and the 
kinematics of the robot. Inverse kinematics is used to obtain the joint positions 
required for the desired end-effector position and orientation [1]. Those 
transformations involve three different representation spaces: actuator space, joint 
space and cartesian space. The relationships between those spaces will be 
established here, with application to an ABB IRB1400 industrial robot (Figure 
2.4). The discussion will be kept general for an anthropomorphic" manipulator with 
a spherical wrist’. 


JE 
*-/ Joint 1 
Spherical Wrist : 


é 
Joint 3 


Joint 6 


uu aiydiowodoijuy 


Figure 2.4 ABB IRB1400 industrial robot 


! An anthropomorphic structure is a set of three revolute joints, with the first joint 
orthogonal to the other two which are parallel 
7A spherical wrist has three revolute joints whose axes intersect at a single point 
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Table 2.1 Denavit-Hartenberg parameters for the IRB1400 


where d is an extra length associated with the end-effector 


Table 2.2 Workspace and maximum velocities for the IRB1400 
Joint 


*115? to -115° 
16 1::3007 to -300° 


Figure 2.5 represents, for simplicity, the robot manipulator axis lines and the 
assigned frames. The Denavit-Hartenberg parameters, the joint range and velocity 
limits are presented in Tables 2.1 and 2.2. The represented frames and associated 
parameters were found using Craig's convention [1]. 


2.2.1 Direct Kinematics 
By simple inspection of Figure 2.5 it is easy to conclude that the last three axes 
form a set of ZYZ Euler angles [1,2] with respect to frame 4. In fact, the overall 
rotation produced by those axes is obtained from: 

1. rotation about Z,4 by 0, 

2. rotation about Y”.“Z”: by 6, 

3. rotation about Z'42Z'', by 05? 


which gives the following rotation matrix, 


3 Y”, corresponds to axis Y, after rotation about Z, by 0, and Z’’, corresponds to Z, after 
rotation about Y '47Z'; by 05 
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Figure 2.5 Link frame assignment 
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Reuter 7 Rz (04) R y4(05) R 74 (86) = 


C4 “S4 0 Cs 0 Ss 1166 —S6 0 
sy c4, Of O 1 Olj|sg ce 0 (2.1) 
[0 0 1İİ-s, 0 cə İİl0 o 1 


It 


€4€506 “S486  —C4€586 —S466 C485 ni no Ts 
$4€5Cg tC48g  —S4C586 t C4Cg S485 |—] T2] To? 193 =R 


7$50€6 $586 e5 Bı 32 I3 


The above rotation matrix R, in accordance vvith the assigned frame settings, 
should verify the following two equations: 


10 0 
R3=|0 0 -I|R 

01 0 
R(9, - 0) - R£ (2.2) 


The values of 04, 05 and 6 can be now obtained. Comparing rış with Təş 
(considering ss # 0) results in, 


04 =A tan 2(r,13) (2.3) 
Squaring and summing rış and rəş and comparing the result with r33 gives, 

9, =A tan Ayr 412, 74) (2.4) 
if a positive square-root of riş +13; is chosen: this assumption limits the range of 05 
to [0,7]. 


Using the same argument now considering elements r3; and r3; the following is 
obtained for 06: 


06 =A tan 2(r35 ,7131) (2.5) 


For 65 € [-7,0] the solution is: 


04 =A tan2(-153,-1]3) 
05 =A tan 2(— ti IA ,I33) 


06 =A tan 2(—13 , 131) (2.6) 


The IRB1400 is an anthropomorphic manipulator with spherical wrist. The 
anthropomorphic structure of the first three joints is the one that offers better 


42 Industrial Robots Programming 


dexterity to the robot manipulator. The first three joints are used to position the 
wrist. The orientation of the wrist is managed by the wrist spherical structure, 
which is also the one that gives higher dexterity. Using the link transformation 
matrix definition derived at [1], 


ci m. 0 8i. 
TH = $j€0;. CjCQi; —S0j, “SOLi-idi Q.7) 
S$jSQ;.j €jSQj,  COj, C0xi--idi 
0 0 0 1 


the direct kinematics of the ABB IRB1400 robot manipulator can be easily 
obtained (as presented in Figure 2.6). 


Cc; S) 0 0 —-$5 —C€5 0 ay C3  -83 0 az 
s; ¢ 0 0 0 0 -1 0 S cz; 0 0 
To -I9 8 qT) = 12 = 3 €3 
0 1d ce) -s) 0 0 0 0 1 0 
L 0 0 I 0 0 0 1] 0 0 0 1 
€4 “SA 0 a3 | [cs -s, 0 0 
0 0 -1 -d 0 0 10 
Ti- 31 T$- 
$4 Cy Ü 0 “Ss ~Cs 0 0 
(0 0 1 | 0 0 01 
E —$6 0 0 ] İ —cis2 m2 EI ajc; 
0 0 -l -d —S189 —SiC cy as 
"s 6| T9 -| 912 192 1 448} 
S6 C6 0 0 Co —S2 0 di 
LO 0 0 1 | i 0 0 0 1 
İ -c1823 C1093 Sp QE S2 Faicı 
T$ = 781823. 7810233 Cy 7825885 +8] 
623 -8$3 0 ane, *dj 
| 0 0 0 1 
7€|$23€4 *$84  C185384 *S|€4. CyCQ3 d4€j€53 — 8361823 —82€]82 aC] 
To = —8,823€4 —€|84 8182384 —CyCq $4093 43,023 — 8381823 — 823182 +aySy 
ES 
€23€4 C384 $23 d4353 ta3€53 tà5€2 * dj 
L 0 0 0 l 
[46566 — 8456 —€4€586 “S4C6 C485 d 664855 maş 
Da $566 “5586 -€5 —deeş -d4 
HES 


S4C€5€g t C486 —S4C5Sg TC466 S485 dgs485 
0 0 0 1 
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0 
€5€6 58g S85 dgsş ni H2 H3 Px 
S c 0 0 Dj E ry Ü : 
Té 4| 56 6 and T2 =| 12! 22 E3 Py with, 
785€6 8586 €5 dgcs Bı ho 53 pz 
0 0 0 1 0 0 O0 1 


Tir.” ((Sisa - C1823C4)€5 - €1€2385)C6 + (C182384 + S1C4)86 

riz.” ((-8184 + €1823C4)C5 + €102385)86 + (C182384 + $1C4)Cs 

113 = (-C1S23C4 + 8184)85 + C1C23€5 

121 = ((-81823C4 - C184)€5 ~ $1C2385)C + (8182384 - C1C4)S6 

I22 = ((S1823C4 + C184)C5 + $1€2385)85 + ($182384 - C1C4) Cg 

I2 = (-81823C4 - €184)85 + S1C23€5 

131 = (C23C4C5 - 82385)C6 - C238486 

I32 = (-023€4€5 + $2385)86 - C2384C6 

E = C23C485 + 8535 

Px = ((-€1823C4 + 8184)85 + C1C23C5)dg + d4€1€23 - 8301823 -AQC 182 + aicı 
Py = ((-81823€4 - C1S4)85 + $1C23C5)dg  da81€23 - 4381823 - 828182  à181 
P z= de(€23C485 + 82305) + d4823 + A3C23 * AQC2 + di 


Figure 2.6 Direct kinematics of an ABB IRB 1400 industrial robot 


Having derived the direct kinematics of the IRB 1400, it’s now possible to obtain 
the end-effector position and orientation from the individual joint angles 
(8;,02,03,04;05,0¢). 


2.2.2 Inverse Kinematics 


Inverse kinematics deals with the problem of finding the required joint angles to 
produce a certain desired position and orientation of the end-effector. Finding the 
inverse kinematics solution for a general manipulator can be a very tricky task. 
Generally they are non-linear equations. Close-form solutions may not be possible 
and multiple, infinity, or impossible solutions can arise. Nevertheless, special cases 
have a closed-form solution and can be solved. 


The sufficient condition for solving a six-axis manipulator is that it must have three 
consecutive revolute axes that intersect at a common point: Pieper condition [5]. 
Three consecutive revolute parallel axes is a special case of the above condition, 
since parallel lines can be considered to intersect at infinity. The ABB IRB 1400 
meets the Pieper condition due to the spherical wrist. 


For these types of manipulators, i.e. manipulators that meet the Pieper condition, it 
is possible to decouple the inverse kinematics problem into two sub-problems: 
position and orientation. A simple strategy [1,2] can then be used to solve the 
inverse kinematics, by separating the position problem from the orientation 
problem. Consider Figure 2.5, where the position and orientation of the end- 
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effector is defined in terms of p and R2 -[n s al. The wrist position (py) can be 
found using 


Pw = p-dea (2.8) 
It is now possible to find the inverse kinematics for 0,0, and 0, and solve the first 
inverse kinematics sub-problem, i.e, the position sub-problem. Considering Figure 


2.7 it is easy to see that 


9, =A tan2(puy Dux) “ (2.9) 


Once 0, is known the problem reduces to solving a planar structure. Looking to 
Figure 2.7 it is possible to successively write 


Pwxi Pu +Pay (2.10) 


P wai = Pwe “di (2.11) 

Pwxt! = Pwxi 7 81 (2.12) 

Pwyt' P wyl (2.13) 

Pwzi' = Pwzl (2.14) 
and 

Pwxi' = 78282 +4xC23) (2.15) 

Pwal' 782€2 +4 S23" (2.16) 


^ Another possibility would be 0, 2 x c tan 2(pyy,P yx) if we set 0; -> x - 0; 
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Puy Xo 


Figure 2.7 Anthropomorphic structure 
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Squaring and summing equations (2.15) and (2.16) results in 
Pour +P wal’ m a? aş T*858,85 
which gives 


2 2 2 2 
sy = Pwxl' Pyer 82 78x 
2a5a, 


Setting cy - £J/1-s3 the solution for 6”) will be 


05 = A tan 2(s3.,¢3:) 
03 = 0';--A tan(a3 /d4) 


(2.17) 


Q.18) 


(2.19) 


Now, using 0^; in (2.15)-(2.16) results in a system with two equations with s; and 


c; unknowns: 


Pwxt' 785€? F.a, (C2¢31 — 89831) 
Pwal! = 4282 +4, (8263 $305) 


Solving for s; and c; gives 


2 —(a2 *taySy)Dwxr *AyCypywzr 


2 
a? *a? *2a5a,8» 


eye (ag +4 x83')P wal! +4 C3!) wal! 
2 


a? val *2a58,83» 


and the solution for 05 will be 


0, =A tan 2(s5,c2) 


(2.20) 


(2.21) 


(2.22) 


(2.23) 


To solve the second inverse kinematics sub-problem (orientation), i.e., to find the 
required joint angles 04, 6; and Öç corresponding to a given end-effector 


orientation R2, we simply take advantage of the special configuration of the last 


three joints. Because the orientation of the end-effector is defined by R$, it's 


simple to get R2 from, 
R$ -(R$) R$ (R3) RG 


which gives 


(2.24) 
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701823 —8,823 €ə3 İlar 812 

3 
Rs“ -Cicə —81€533 893 daşı aşı 
8j mə 0 İlası 832 


vvith 


Ti” -C)S23a)) - 81823321 + C23831 
I13 = 701823813 - 81823323 + €23835 
133 = $1815 - C1823 
Tər.” -€1C2381; - 81023821 - 823831 
Tar” 8184, - Caz 


ni 12 Hə 
mİTy Tə 15 (2.25) 
Ij DB I3 


T12 7 -C1823842 - 81823822 + 023332 
T23 = -01C23813 - 81023823 - 823233 


I2? = -01C23812 - 81023822 - $2332 
132 = 81842 - Caz? 


It is now possible to use the previous result for the ZYZ Euler angles to obtain the 


solutions for 0;, 05 and 0; . 
For 65 € (0, zl the solution is 


04 = A tan 2(133 5113) 
05 =A tan 2(iİriğ +133 123) 
06 =A tan 2(—r99 , 19) 

For 05 € [-1,0] the solution is 


0, =A tan 2(—153,-113) 


05 = Atan2(—- 1 + rà 123) 
06 =A tan 2(125 ,-11) 


2.3 Jacobian 


(2.26) 


(2.27) 


In this section, the equations necessary to compute the jacobian of the ABB 
IRB 1400 industrial robot are presented and the jacobian is obtained. Nevertheless, 
the discussion will be kept general for an anthropomorphic robot manipulator. In 
the process, the equations that describe the linear and angular velocities, static 
forces, and moments of each of the manipulator links are also presented and the 
corresponding developments applied to the selected robot. 


The jacobian of any robot manipulator structure is a matrix that relates the end- 
effector linear and angular Cartesian velocities with the individual joint velocities: 
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MH = (0) (2.28) 
Ww 


where J(8) is the jacobian matrix of the robot manipulator, 0 = b, 02 “Ön İs the 
joint velocity vector, v - [vi və, və” is the end-effector linear velocity vector, and 


vv -2[wi, wa, wa]! is the end-effector angular velocity vector. 


The jacobian is an nxm matrix, where n is the number of degrees of freedom of the 
robot manipulator and m is the number of joints. Considering an anthropomorphic 
robot manipulator with a spherical wrist, the corresponding jacobian will be a 6x6 
matrix. Basically there are two ways to compute the jacobian: 


1. By direct differentiation of the direct kinematics function with respect to 
the joint variables. This usually leads to the so-called analytical jacobian, 


“1 (2.29) 


where p is the time derivative of the position of the end-effector frame 


with respect to the base frame, $ is the time derivative of the orientation 
vector expressed in terms of three variables (for instance, ZYZ Euler 
angles). Obviously, p is the translational velocity of the end-effector and 


$ is the rotational velocity. 
2. By computing the contributions of each joint velocity to the components of 
the end-effector Cartesian linear and angular velocities. This procedure 


leads to the geometric jacobian. 


Generally, the analytical and geometrical jacobian are different from each other. 
Nevertheless, it is always possible to write 


w 2 T(0).$ (2.30) 


where T is a transformation matrix from $ to vv. Once T(4) is given, the analytical 
jacobian and geometric jacobian can be related by 


I 0 |. . 
Vm l .. x T,(6)x (2.31) 


which gives 


15 T, G)JA (2.32) 
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Here the geometric jacobian will be calculated, because in the process the linear 
and angular velocities of each link will also be obtained. Nevertheless, the 
analytical jacobian should be used when the variables are defined in the operational 
space. 


First the equations for the link linear and angular velocities and accelerations [1,2] 
will be obtained. Associating a frame to each rigid body, the rigid body motion can 


be described by the relative motion of the associated frames. Consider a frame (B) 
associated with a point D (Figure 2.8). 


(Bj 


(A31 


Figure 2.8 Describing point D relative to a stationary frame 


The position vector of point D in frame (B) is PD and the relative velocity of D 
described about an arbitrary stationary frame {A} is [6], 


AVp 2 ^Vg - $R "Vb (2.33) 


If the relative motion between {A} and {B} is non-linear then (2.33) is not valid. 
The relative motion between two frames {A} and {B} has generally two 
components: a linear component “Va and a non-linear component (the angular or 
rotational acceleration) “Op as in (Figure 2.9). 
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(A) 


Figure 2.9 Relative motion between two frames (A) and (B) 


In that general case it can be written [1,6,7], 
^yp 2 ^v, 4 4R "Vp -AQpx AR PD (2.34) 


where “Vy is the linear velocity of the origin of frame {B} about frame (A), 3R 
Py is the linear velocity of point D about frame (B) expressed in terms of {A} 
(i.e., AR PV5 “(”Vp) ), “Og x AR PD- ^Og x ^D is the linear velocity of point 


D about (A) expressed in terms of (A) as the result of the angular velocity ^Og of 
{B} about {A}. 


If D is stationary in (B) ("Vp = 0) and the origins of (A) and (B) are coincident, 
i.e., the relative motion of D about {A} is only due to the rotation motion of (B) 
about (A) described by ^Os , then “Vp = “Ox x AR PD. This equation can also be 
obtained by differentiation of 


‘p= R ?D (2.35) 
which yields 

^D « 4R ®p+ 4r PD (2.36) 
or since in this special case 4R P D “ 0, 


^yp = öR PD (2.37) 
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Substituting in (2.37) PD = 4R^! ^D results in 

^yp- AR Bg ^D (2.38) 
Because BR is an orthonormal matrix, we can write [1,7], 

BEAR üs (2.39) 
where 4S is a skew-symmetric matrix associated with 4R . 
Using (2.39) in (2.38) gives 


^yy = 4S ^D (2.40) 


The skew-symmetric matrix 4S defined in (2.39) is called angular velocity matrix. 


Writing S as 
0 -O, O, 
Se İO, 0 -Q, (2.41) 
-Q, OQ, 0 


[om 
Q= |O0, (2.42) 
Oz 
results in 
0 -o, O,|[D,] [-2,D,+9,D, 
SD=; 9, 0 -O, .|D| 7| QG;D,-O,D, |=QxD (2.43) 
-Q, 0, 0 ||D,] |-0O,D,*O,D, 


where D =(D, , D, , D, )' is a position vector. The vector Q associated with the 
angular velocity matrix is called an angular velocity vector. Using (2.43) and 
(2.40) gives 


AVD = ^Os x ^D (2.44) 
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Considering now the linear and angular accelerations of each link, it's possible to 
write by direct differentiation of (2.34), 


^Vp-^Vg-(8R PVp) *^Ogx BR PD e ^Og x (GR PD) (2.45) 
or since, 
(AR "Vpy = 8RP, p+ ^O, x SR PVp 
and 
(SR PD) = $R "Vp + ^Og x BR PD, 
^Vps^Vgt BR PVp*2^0s x BR "Vb- 
£^05gx &R PED +4Q, x AQ, x 4R PD) (2.46) 


The above equation is the general equation for the linear acceleration of point D 
about {A} and expressed in terms of {A}. If PD is a constant vector (like in 
robotics applications) then equation (2.46) simplifies to 

^Vp-s^Vs--^ Qgx BR PD ^Og x (^Og x öR PD) (2.47) 
because ®Vp = P V p =0. 
If we consider a third frame (C), with “Op being the angular velocity of (B) about 
(A) and "Oç the angular velocity of (B) about (C), then the angular velocity of 
(C) about {A} is, 

^Qc “Op *- $R POc (248) 

Taking the derivative of (2.48) results in 


^Qcs^Qg*(8R "Qey -^ Og BR P Oc ^Os x FR "Qe (2.49) 


This is a very useful equation to compute the angular acceleration propagation 
from link to link. 


Let's apply this to a robot manipulator. As mentioned before we will consider only 
rigid manipulators with revolutionary joints, with the base frame as the reference 
frame. 
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| Axis i 


Figure 2.10 Linear and angular velocity vectors of adjacent links 
The angular velocity of link (i+1), expressed in terms of {i}, is given by” 
Wis ='wit LR Özü "I Zu (2.50) 


It is equal to the angular velocity of link (i) plus the angular velocity of joint (i*1) 
about Z;,; expressed in terms of {i}. 


Multiplying both sides of (2.50) by '*!R results in the angular velocity of link 
(i+1) expressed in terms of {i+1}, 


Wie HIR Wie = PER wit Oui P Zu (2.51) 


5 Note that wi-1 = 0Qi*-1 and that iwi*l is the same quantity expressed in terms of {i}. 
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The linear velocity of the origin of {i+1}, expressed in terms of {1}, is given by 
View = vit wi x Pa (2.52) 


It is equal to the linear velocity of the origin of {i} plus a term that results from the 
rotation of the link (i+1) about Z;4,, The same solution can be obtained from (7) by 
making 'P;,; constant in {i}, i.e., by making "ii = 0. 


Multiplying both sides of (2.52) by ""İR we get the linear velocity of link (i+1) 
expressed in terms of (1*1) 


yi m "İR (vit wi x Pu) (2.53) 


Applying (2.51) and (2.53) from link to link, the equations for "w, and "v, (where n 
is the number of joints) will be obtained. The equations for ^w, and °v, can be 


obtained by pre-multiplying "w, and "v, by ÜR: 


ws OR "vn (2.54) 

ya OR "v. (2.55) 
It's also important to know how forces and moments distribute through the links 
and joints of the robot manipulator in a static situation, i.e., how to compute the 
forces and moments that keep the manipulator still in the various operating static 
configurations. Considering the manipulator at some configuration, the static 
equilibrium is obtained by proper balancing of the forces and moments applied to 
each joint and link, i.e., by cancelling the resultant of all the forces applied to the 
center of mass of each link (static equilibrium). The objective is to find the set of 


moments that should be applied to each joint to keep the manipulator in static 
equilibrium for some working configuration (Figure 2.11). 


Considering, 


f; = force applied at link (i) by link (i-1) 
n, = moment in link (1) due to link (1-1) 


the static equilibrium is obtained when 

f-fa-0 and 'nj- ‘nit - Pa x “fr = 0 (2.56) 
i.e., when, 

fi fe (2.57) 


and 
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i 


ni = ni + Pa, x fa (2.58) 


Figure 2.11 Static equilibrium: force balancing over link (i) 


Writing the above equations in their own reference frame gives 


f= İR "If (2.59) 


ini2 biR: "nj; + Pu; xf (2.60) 


To compute the set of joint moments that hold the manipulator in static equilibrium 
we must obtain, for each joint (i), the projection of 'n; over the joint axis 


z; 2n; İZ, (2.61) 
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Returning to the jacobian, from (2.54)-(2.55) it's possible to write 
wi, = wi İR. (Oi Zi) (2.62) 


View m vi wi x OPA, (2.63) 


Using (1) and (2.62)-(2.63) the i" column of the jacobian can be found to be 


Opi 
ali 2 . (2.64) 


Zi 


Applying (2.62), (2.63), and (2.64) to the IRB1400 industrial robot, the equations 
presented in Figure 2.12 are obtained. 


0 -alsığ, $05 
000 *woz0 ?v, «0 w=] 0 6 -c:ğ 
07 Wo = y= Ww, = Və el 40,9) W5z|-C€jU5 
ó, 0 ó, 
{a 28182 —a,81)0; -a2cic202 $1(05 +83) 
: ü 279 
V3 =| (a,c) “aşcisə))0) -aşeəsi02 ws l-ci (02 +83) 
428709 9; 
EN 


(a282 —4 +3893 —d4€53)$,0; — (2505 +d 4823 —a3€ə3 )ci02 —(d 483 *a5€53)€10 
(ay -a2$2 * d4053 —a5523)610; - (3262 +4893 +3623 8102 — (d4$23 +43023)8195 
(44623 “382 ~ A983 )O2 * (d4€55 —a5523)05 

si (05 £03) ^ c;02504 
Ow, el-c, (85 +03) 5,0250, 
0, 52304 


ES 


(4282 “ai * 43853 -d4€23)8,0, — (a5€2 “44823 —a3€53)0102 —(d 4893  a5€23)c105 
(aj —4282 *d4€53 -a5823)010; - (a5€5 * d4s23 +43C23 )s102 —(d4525 *a5€23)8105 
(d4€53 “33823 7828$2)05 “(44623 —a3853)03 


2 ? 
si (02 05) * 0,025304 t (c1$2384 “S)64 )05 vg(x) 

ü an" " " 
Ws -c, (0, 03) 5102304 t (8189384 —-€4104)05 Ve vs (y) 

, , $ 
01 $5304 —€23$405 vg(z) 


0 2 
Vo(X) = ((a282 - a) 33823 - dyco3)8, + do((S1823¢4  €184)85 - Sicz3cş)) 9) + 
+ ((-a2€2 - d4823 - à3€23) - de(c23c485 + $236s))c182 + (ci(-das25 - a3623) - de(c23c485 + 


+ 85365)) 03 + de(Sic48s + 018485823) 04. + de(S1C584 - €102385 - €10405823) 05 
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vey) = (ai - aşsə + dac23 - a3523)1 ((-c1*s23*c4-s1*s4)*s5--c1*c23*c5)*d;) 0, - 
((a2c2 + das23 + a3C23) + do(C23C48s + S23Cs))S1 02 - ((das23 + aşcəa)si + 
+ de$i(C23C48s+ $23Cs)) 03 + do(Sz381S485 - C1C4Ss) 04. - de(Cy38)85 + CiS405 + 


+ $1C4€5823) 05 


" f 
vetz) = ((C23C5 - S23C48s)de + d423 -a3823 -a282)05 + ((C23C5 - S23C485)dg +dycz3 - 


43823) 83 - $sS4C23d6 — ^ (C23€5C4 - $sSz3)de 05 


Ogee 
81 (05 +03) 0102304 + (6482384 +8104 )O5 +((—C1873C4 +8184 )S5 t 0102305 )06 
— C1 (87 +03) +81C304 + (818384 —0,04)05 —((S1823C4 +0184 )S5 —S1€23c5 )06 
L 0) 8230, ~C738495 t (€530485 18235 )06 
41- 
b 
(a282 “ai 83823 —d4€53)81. “(a cə 4483 -a3€53)€; “(4483 ta3€53)e; 0 
(aj —a 782 +d4¢73 “a3sə3 sı — (aze? 144823 +43C73)8; —(d4823+43C23)8; 0 
0 d4€23 783823 —a282 d4€23 “43823 0 
0 S) S| €1€53 
0 —C) miri S1C23 
L 1 0 0 $53 
455,9; —aj8| “ace 0 Ju 12-13 ha Sts Sie 
4,0 “a2ci82 “ac 0 Ja) J22 723 124 125 J56 
07- 0 5... Vi Baz 133 134 Jos İs 
0 s SI Ya 142 143 144 14: Jag 
0 “Cı —cı 1: 132 13 Isa Jos 456 
L 1 0 0 Jor Jo Jes Jes Jos Jee | 


Ji; = (a2*s2 - al + a3*s23 - d4*c23)*sl + d6*((s1*c4*s23 + cl *s4)*s5 — 
- s1*c23*c5); 

Ji; = ((-a2*c2 - d4*s23 - a3*c23) - d6*(c23*c4*s5 + s23*c5))*cl; 

Ji; = cl *((-d4*s23 - a3*c23) - d6*(c23*c4*s5 + s23*c5)); 

Jia = d6*(s1*c4*s5 + c1*s4*s5*s23); 

Jis = d6*(s1 *c5*s4 - c1*c23*s5 - c1*c4*c5*s23); 

Jis = 0; 


Jo, = (al - a2*s2 + d4*c23 - a3*s23)*cl + 
+((-c1*s23*c4+s1*s4)*s5+c1 *c23*c5)*d6; 
J22 = - ((a2*c2 + d4*s23 + a3*c23) + d6*(c23*c4*s5 + s23*c5))*sl; 
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J535 = - (44723 + a3*c23)*s1 - d6*s1*(c23*c4*s5 + s237c5), 
J34 = d6* (s23*s1*s4*s5 - c1*c4*s5); 

J5s = - d6*(c23*s1*s5 + c1*s4*c5 + s1*c4*c5*s23); 

426” 0; 


Ja = 0; 

J3; = (c23*c5 - s23*c4*s5)*d6 + d4*c23 -a3*s23 -a2*s2; 
J33  (c23*c5 - s23*c4*s5)*d6 +d4*c23 -a3*s23; 

J34 = - 85*s4*c23*d6; 

Jas = (c23*c5*c4 - s5*s23)*d6; 


436” 0; 
Jai = 0; 
J4 € sl, 
Ja 7 sl; 
J44 =cl *c23; 


Jas = c1*s23*s4 + s1*c4; 
Jag (- c1*s23*c4 + s1*s4)*s5 + c1*c23*c5; 


Js = 0; 
dz” - cl; 
Js3 = - cl; 
J54 7 s1*c23; 


J5s = $1 *823*84 - c1*c4; 
Ise = - ((S1*s23*c4 + c1*s4)*s5 - s1*c23*c5); 


Jai * 1; 

Jo; = 0; 

Jes 0; 

Jg4 = $23; 

Jos = -c23*s4; 


Jas = c23*c4*s5 + s237c5, 


Note: These calculations were made in MatLab using the symbolic Toolbox. 


Figure 2.12 Linear and angular velocities, jacobian matrices $3, 27 and QJ 


2.4 Singularities 


If the objective is to use the differential kinematics equation (2.28) for simplicity 
and efficiency, then it’s necessary to deal with the singularities of the jacobian. The 
differential kinematics equation maps the vector of joint velocities 
q-là d» 43 da ds döl with the end-effector twist vector V = V wif : 
This mapping is seriously affected when the jacobian is rank-deficient (kinematics 
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singularities), because in those situations the mobility of the robot is reduced, the 
inverse kinematics may show infinite solutions, and (because the jacobian 
determinant may take very small values near singularities) small task space 
velocities may cause very large joint velocities [2]. So, to control the robot 
manipulator it is necessary to find all singular configurations and design a scheme 
to identify a singular configuration approach. 


In order to find all the singular points of the ABB IRB 1400 anthropomorphic 
industrial robot, which has a very simple kinematic structure, a scheme will be 
used that separates the arm singularities and the wrist singularities. By dividing the 
jacobian into four 3x3 blocks it can then be expressed as 


J J 
0 uo Jo 
pe (2.65) 
i b 2 


Now, looking to all the elements of Ji; (Figure 2.12) it is clear that det(J,2) 
vanishes making dş-“0. That is equivalent to choosing the origin of the end-effector 
frame coincident with the origin of axis 4 and 5, ie., making py = p. Since 
singularities are a characteristic of the robot structure and do not depend on the 
frames chosen to describe kinematically the robot, this procedure is allowed. It's 
possible then to write 


det(J) = det(J;;) *det(J2;) (2.66) 


The robot’s singular configurations are the ones that make det(J) = 0 which means 
from (2.66) 


det(J;j) * 0. or det(7522 2 0 (2.67) 


Solving the first equation leads to the so called arm singularities and solving the 
second leads to the wrist singularities. 


Wrist Singularities 
The wrist singularities can be found just by analyzing the structure of det(J7;): 


deo)» dez, z, z,)- 


C623. 182984 — C104 — (8184 — €1823€3)85 + €1€23€5 (2.68) 
det sjc23 s,925384 — CiC4 — (8482304 + €194)85 + S€23€5 


823 7 €2384 C23€485 "^ 823€5 


The above determinant is non-null if the column vectors of J,2 (which correspond 
to za, Zs, and zə) are linearly independent, i.e., the singular configurations are the 
ones that make at least two of them linearly dependent. Now, vectors z4 and zs are 
linearly independent in all configurations, and the same occurs between zs and zs. 
This conclusion is easy to understand looking to (2.68) and/or remembering that z, 
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is perpendicular to zs, and zs is perpendicular to zş in all possible robot 
configurations. A singular configuration appears when z4 and zs are linearly 
dependent, i.e, when those axis align with each other, which means s;=0 from 
(2.68). Consequently the wrist singular configurations occur when, 


0070 or 67m (2.69) 


The second condition (0; = x) is out of joint 5 work range, and because of that is of 
no interest, i.e., the wrist singularities will occur whenever 05 = 0. 


Arm Singularities 
The arm singularities occur when det(J;;) = 0 making again p = py => dş =0, i.e., 
when 


(4782 —8, +3823 — d4c23)9)  —(a5€5 ^ d4$53 —a3€53)€] ~ (d4823 t 83€23)€1 
deti (aj — 8585 t d4€55 - 3853)C| - (a2c2 t d483 + 43C23)€; - (44823 + 43093 )C; =0 


0 d4€23 — A383 —a282 d4055 —aş8z3 
(2.70) 

Solving (2.70) gives 

-à5(d4c, —a383 )(a3823 —d4c23 +4282 ~a,;) =0 (2.71) 
which leads to the following conditions: 

—8383 +d4c3 =0 

and/or 
43893 —d4023 +a282 “aş “0 (2.72) 


The first condition leads to 03 = arctg & . The elbow is completely stretched out 


a3 
and the robot manipulator is in the so called elbow singularity. This value of 93 is 
out of joint 3's work range, so it corresponds to a non-reachable configuration, and 
because of that is of no interest. 


The second condition corresponds to configurations in which the origin of the wrist 
(origin of axis 4) lies in the axis of joint 1, i.e., lies in zı (note that zı is coincident 
with zo). In those configurations, the position of the wrist cannot be changed by 
rotation of the remaining free joint 0, (remember that an anthropomorphic 
manipulator with a spherical wrist uses the anthropomorphic arm to position the 
spherical wrist, which is then used to set the orientation of the end-effector). The 
manipulator is in the so called shoulder singularity. 
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In conclusion, the arm singularities of the ABB IRB 1400 industrial robot are 
confined to all the configurations that correspond to a shoulder singularity, 1.e., to 
configurations where a4s5; —d4€53 *a5s5 —a, =0. 


2.4.1 Brief Overview: Singularity Approach 


As already mentioned, the solutions of the inverse kinematics problem can be 
computed from 


q=J'@V (2.73) 


solving (2.28) in order to q . With this approach it’s possible to compute the joint 
trajectories (q, q ), initially defined in terms of the end-effector wrist vector V and 
of the initial position/orientation. In fact, if q(0) is known it's possible to calculate: 


à (t) from: q(t) = J- (0)V(t) 


and 


t 
q(t) from: a(t) = 4(0) + [à(o)do. (2.74) 
0 


Nevertheless, this is only possible if the jacobian is full rank, i.e., if the robot 
manipulator is out of singular configurations where the jacobian contains linearly 
dependent column vectors. In the neighborhood of a singularity, the jacobian 
inverse may take very high values, due to small values of det(J), i.e. in the 
neighborhood of a singular point small values of the velocity in the task space (V) 
can lead to very high values of the velocity in the joint space ( q ). 


The singular value decomposition (SVD) of the jacobian [3,8-10] is maybe the 
most general way to analyze what happens in the neighborhood of a singular point; 
also it is the only general reliable method to numerically determine the rank of the 
jacobian and the closeness to a singular point. With the inside given by the SVD of 
the jacobian, a Damped Least-Square scheme [9] can be optimized to be used in 
near-singular configurations. The Damped Least-Square (DLS) scheme trades-off 
accuracy of the inverse kinematics solutions with feasibility of those solutions: this 
trade-off is regulated by the damping factor &. To see how this works, let's define 


the DLS inverse jacobian by rewriting (2.28) in the form 


(7 “6714 - v (2.75) 


where 6 is the so-called damping factor. Solving (2.75) in order to q gives 
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qa «gp Tv - lv (2.76) 


with Ja, being the damped least-square jacobian inverse. The solutions of (2.76) 
are the ones that minimize the following cost function [2,9,11]: 


: il , : l,2.T. 
ed) = (V - (V - I) 67474 (2.77) 
resulting from the condition 
, .2 21:12 
min(]v — al 22d (2.78) 


The solutions are a trade-off between the /east-square condition and the minimum 
norm condition. It is very important to select carefully the damping factor 6 : small 
values of & lead to accurate solutions but with low robustness to the singular or 
near-singular occurrences ( — high degree of failure in singular or near-singular 
configurations), i.e., low robustness to the main reason to use the scheme. High 
values of & lead to feasible but awkward solutions. 


To understand how to select the damping factor €, in the following the jacobian 


will be decomposed using the SVD technique. The SVD of the jacobian can be 
expressed as 


6 
] 2 UXV" = Yoiuv] (2.79) 
1 


where o, > 02>... > o, > 0 (r= rank(J)) are the jacobian singular values (positive 
square roots of the eigenvalues of J*J), v; (columns of the orthogonal matrix V) are 
the so-called right or input singular vectors (orthonormal eigenvectors of J'J) and 
uj (columns of the orthogonal matrix U) are the so-called left or output singular 
vectors (orthonormal eigenvectors of JJ"). The following properties hold: 


R(J) = span (ui, ..., uj? 
N(J) = span (vii, ..., Vo} 


The range of the jacobian R(J) is the set of all possible task velocities, those that 
could result from all possible joint velocities: RJ)  (Ve8”: V = Jd for all 
possible d € 91$). The first r input singular vectors constitute a base of R(J). So, if 
in a singularity the rank of the jacobian is reduced then one other effect of a 
singularity will be the decrease of dim[R(J)] by eliminating a linear combination of 


$ The span of fa), .... an) is the set of the linear combinations of a), ... ap. 
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task velocities from the space of feasible velocities, i.e., the reduction of the set of 
all possible task velocities. 


The null space of the jacobian N(J) is the set of all the joint velocities that produce 
a null task velocity at the current configuration: N(J) = (q eR: Jq = 0). The last 
(6-r) output singular vectors constitute a base of N(J) So, in a singular 
configuration the dimension of N(J) is increased by adding a linear combination of 
joint velocities that produce a null task velocity. 


Using the SVD of the jacobian (2.78) in the DLS form of the inverse kinematics 
(2.75) results in 


6 o: 
q=> ——— viulv (2.80) 
1 + 


The following properties hold: 


RGias) = Rt - N' (Jy 7 span fui, ..., u,) 
N(Jids) = RQ') = R^ = span (via, ..., Vo} (2.81) 


which means that the properties of the damped least-squares inverse solution are 
analogous to those of the pseudoinverse solution (remember that the inverse 
pseudoinverse solution gives a /east-square solution with a minimum norm to 
equation (2.28)). 


The damping factor has little influence on the components for which o; >> & 
because in those situations 


Oi 1 
zt 2:82 
o? +8? Oj ( ) 


i.e., the solutions are similar to the pure /east-square solutions. 


Nevertheless, when a singularity is approached, the smallest singular value (the r- 

th singular value) tend's to zero, the associated component of the solution is driven 

to zero by the factor T and the joint velocity associated with the near-degenerate 
$ 

components of the commanded velocity V are progressively reduced, i.e, at a 

singular configuration, the joint velocity along v, is removed (no longer remains in 

the null-space of the jacobian as in the pure Least-Square solution) and the task 


7 Yİ is the pseudoinverse jacobian. 
* Orthogonal complement of the null space joint velocities. 
? Orthogonal complement of the feasible space task velocities. 
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velocity along u, becomes feasible. That is how the damping factor works; as a 
measure or indication of the degree of approximation between the damped and 
pure least-square solutions. Then a strategy [8], initially presented by [12], can be 
used to adjust $ as a function of the closeness to the singularity. Based on the 
estimation of the smallest singular value of the jacobian, we can define a singular 
region and use the exact solution (5-0) outside the region and a damped solution 
inside the region. In this case, a varying & should be used (increasing as we 
approach the singular point) to achieve better performance (as mentioned the 
damped solutions are different from the exact solutions). The damping factor 6 can 
then be defined using the following law modified from [9] 


0 
Ane a - 
£ 


where E2.. and n are defined by the user to shape the solution to his needs, £ 


defines the size of the region and G¢ is the estimate of the smallest singular value. 


The estimate is done using a recursive algorithm originally presented at [13] and 
later extended by [14] to estimate not only the smallest singular value but also the 
second smallest singular value. This procedure avoids estimation inaccuracy due to 
the cross of the two smallest singular values, when the manipulator approaches 
both the wrist and the shoulder singularity. The algorithm is as follows: 


Suppose we have estimates of the two last input singular vectors v5 and vg with 


$5 & vs and İl”, =1 
96 evs and [os] 21 (2.84) 


The estimate v4 is then use to compute v4 from 
rı + ey, = % (2.85) 
Then the estimate 62 is computed from 


66 “5” (2.86) 
[s 


and the initial estimate vg is updated using 
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PLI (2.87) 
[| 
The second smallest singular value is computed using the estimate v, from, 
Irr«ei- (62 +22 bot bi = 0, (2.88) 
Then the estimate 62 is computed from 
62 = pe (2.89) 
I 
and finally the initial estimate v5 is updated using 
ə (2.90) 
Fl 


Special care should be taken vvith the numerical implementation of the DLS 
İnverse kinematics solutions, to correct the numerical drift. Basically a feedback 
term can be used [2,9,15] by making 


Pa-p 
V=VytKe=Vyt+K 5 (nxng esq saxaş) (2.91) 


where K is a positive definite diagonal 6x6 matrix, pa and p are the desired and 
actual position, and the orientation is defined in terms of the desired and actual (n, 
s, a) vectors of the end-effector frame. 


Due to the increase of end-effector errors [11] in the neighborhood of a singularity 
by means of the near-degenerate components of end-effector velocity, the matrix K 
should be corrected using K=pKo , where Ko is a diagonal constant matrix and p is 
the correcting factor. Now, inside a singular region we should use K=0 because in 
some situations the resulting joint velocities can drive the manipulator to reach the 
Joint limits, even if eventually the error will approach zero. When the manipulator 
is sufficiently away from a singularity, we should have p=1. So, generally we 
define p as 
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0 Og SE 
- (eco £ € Og € n£ (2.92) 
de (n -1)7:? I : 
1 othervvise 


where n is defined by the user based on self-experience and on test results with a 
particular robotic manipulator setup. 


2.5 Position Sensing 


The IRB1400 uses resolvers [16-19] as position sensors. The drive unit used at this 
robot (manufactured by ELMO AB for ABB Robotics), includes a PM AC 
synchronous motor, both current feedback devices, a brake, and a brushless 
resolver, all assembled at factory ,i.e., they come in one piece [20]. 


A brushless resolver consists of a stator, a rotor and a rotary transformer. The stator 
and rotor windings are distributed in a way that the magnetic flux is distributed as a 
sine wave of the angle of rotation (perfect resolver). The output of a resolver is 
therefore an AC voltage in accordance with the angular position of the shaft. This 
type of position sensor is characterized by its high accuracy output, maintenance 
free brushless design, and immunity to noise, vibration, and shock. Other 
characteristics introduced by highly automated manufacturer production facilities 
include homogeneity in accuracy, transformation ratio, phase-shift, etc. 


These characteristics significantly reduce major sources of error such as: 


l. Amplitude imbalance due to different amplitudes of the resolver output 
signals 

2. Imperfect quadrature due to phase-shift 

3. Inductance harmonic error due to imperfect inductance profiles, i.e., the 
inductance profiles do not follow perfect sine wave as consequence of 
imperfect sinusoidal winding 


Two types of resolvers can be considered (Figure 2.13): Brushless Amplitude 
Output Resolvers (BAOR) and Brushless Phase-Shift Output Resolvers (BPOR)/”. 
Resolvers of type BAOR are excited by an AC voltage to the rotor vvinding and the 
output is obtained from the stator windings in the form sine and cosine voltages 
proportional to the rotation angle 0. Resolvers of the type BPOR are excited by 
sine and cosine voltages to the stator vvindings and the output is obtained from the 


10 Tamagavva Seiki Co. LTD. names these resolvers as BRX and BRT, respectively. 
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rotor winding in the form of a sine voltage with phase-shifted in proportion to the 
rotation angle 0. 


The IRB 1400 uses BAOR type resolvers from the Japanese manufacturer 
Tamagawa Seiki Co. LTD. [19,20]. 


BAOR 


RED/WHT Excitation : Ert-ne=Esin cot 


2 gəl Output Eot.sa=K Ent-R2 Cos 6 
i İsə Eso-sazzK Ent-nz Sin 0 (Normal Typo) 


! “Sa Esz.saz -K Eai-nə Sin @ (Reverse Type) 
(BLKAVHT) 1B K : Transformation Ratio 
BLU YEL 
PRIMARY SECONDARY 
BPOR 
Excitation : Es1-s3=Esin «t 


Co RED/WHT 
pena M Eszs4-E Cos ot 


İsa Output : Ent.&2zK Estisa Cos 0 -K Esz.s4 Sin 6 
zKE Sin (ot-6) (Normal Type) 
(BLKAVHT) Ent.R2zK Est-s3 Cos 6 + K Esə-sa Sin 0 
=KE Sin (wt+6) (Reverse Typo) 
K : Transformation Ratio 


BLU YEL 
PRIMARY SECONDARY 


Figure 2.13 Types of resolvers 


The use of a resolver implies the availability of a resolver to digital converter 
(RDC) and processing circuit [21-23]. The RDC is used to track and convert 
resolver signals to a digital parallel binary word, generally using a ratiometric 
tracking conversion method that improves noise immunity and tolerance to lead 
length (important when the converter is remote from the resolver). The RDC 
circuit uses an RDC along with the necessary interface and signal conditioning 
circuitry. Because noise can degrade significantly the accuracy of the 
measurement, special care must be taken with the driving lines from and to the 
resolvers: the use of shielded twisted pair cabling and isolation amplifiers may be 
needed. 


The basic functional diagram of an RDC is presented in Figure 2.14, where it is 
used data relative to the Analog Devices RDC model AD2S80A. The converter 
works as a type II closed-loop system with the angle $ as a control variable (this 
angle is the current converter estimate of the angle 0). 


Generally, the converter's functioning can be described as follows: First the inputs 
(resolver outputs Esə.sa and Es;.53) are multiplied by cos($) and sin(), respectively, 
at the ratio multiplier. Then the difference between the signals is computed giving 
the ratio multiplier output AC error signal E,, = Aj.K.E.sin(0 — 6).sin(et) , where A; 
is the ratio multiplier gain (fixed at 14.5 for AD2S80A). Second, this error signal is 


synchronously demodulated at the phase sensitive demodulator (PSD), using the 
resolver excitation frequency as a demodulation reference, leaving the error signal 
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Epsp = AK Esin(0—4). The output of the demodulator is a DC voltage 
proportional to the RMS value of the demodulator input: 


2 : ; : ; : : 
*Demodulator Inputgys (for sinusoidal input signals in phase or antiphase 


with the reference signal). Before entering the PSD, the signal passes over an HF’ 
filter (with components selected by the user) to remove any DC offset voltage. 
Then the PSD output passes through the integrator (with components selected by 
the user), whose output signal (proportional to the velocity of the resolver) is fed to 
the voltage controlled oscillator (VCO). The VCO integrates the velocity signal 
and compares the resulting signal with the minimum DC voltage resolution (uses 
two comparators for positive and negative voltages, meaning rotation in the 
positive direction or in the negative direction) and updates the up/down counter by 
producing the counter clock and direction signal. The value of the internal latch 
used to interface with the user is also updated with the counter value. An RDC 
works similarly to a successive approximation type analog to digital converter. 


k.E.sin(vvt).sin(9 AC m 
Ratio LAC» HF filter | Phase Sensitive 
Demodulator 


k.E.sin(wt).cos(9) | Multiplier 


Integrator 


Voltage Controlled Velocity 


Oscilator 


Up/Down Counter 
with reset 


Output to Latch 
Figure 2.14 Resolver to digital converter basic functional diagram 


The RDC returned digital value is generally a 12, 14, or 16-bit binary number 
containing the actual rotation angle. This angle should be mapped to the robot's 
join space. For that, the following guidelines should be used: 


1. Choose an angle data format, i.e., degrees, radians 

2. Account for the resolver offset! ie, the resolver reading when the 
manipulator is in the home position. At that point, we should have 
number. of rotations = 0 and actual angle = 0 


A complete RDC circuit implementation should also save the total number of 
rotations in an 8-bit up-down counter/register. In essence, the circuit should give 
the rotation angle of the motor in the actual rotation and the total number of 
rotations already performed. 


" Usually these values are measured by the robot manufacturer and printed on the robot or 
in the robot documentation. 
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2.6 Actuators: Motors 


Generally the actuators used to move the joints of any industrial robot are motors, 
usually DC permanent magnet (PM) motors or AC PM motors. Other motors can 
be used, including pneumatic or hydraulic servo motors. The IRB 1400 uses three- 
phase synchronous AC PM motors, with six poles (axes 1-3) and four poles (axes 
4-6), manufactured by Elmo AB — Sweden. 


The three-phase synchronous AC PM motor rotating magnetic field is obtained by 
making a three-phase current to flow in the stator coil (Figure 2.15), which has a 
sinusoidal distribution. So, a brushless sine wave PM AC synchronous motor is 
obviously not mechanically commutated (there are no brushes) but instead the 
commutation is done by acting on the three-phase current signals. Nevertheless, the 
commutation position of the motor should be retained, i.e., the resolver reading 
when the motor is at the electrical home position (electrical 0? position) - this value 
is called the commutation offset (COMMOFF). 


The usual procedure to find the commutation offsets 1s as follows: 

1. Turn the motor to the commutating position by feeding a positive constant 
current to the motor 

2. Feed the resolver with the necessary excitation signal (4kHz and 5 Vin: for 
TRB 1400 drives) 

3. Adjust the resolver to +90° (+ 0,5°), i.e, turn to the maximum value on coil 
Y of the resolver with the same phase as the 5V feeding signal. At that 
point we should have: 

Voltage across coil X = 0V 
and 
Voltage across coil Y = input voltage * transformation ratio 


The value of the rotation angle (90 degrees) is the commutation offset. This 
procedure is used with the IRB 1400 drives, so that is why the COMMOFFS are 
constant for all drives (1.570800 radians). For some older robots, like the ABB 
IRB 2000 (up to model M90), the motor and the resolver are separate parts, 
assembled together by the manufacturer without following the above referred 
procedure. So, the COMMOFFS are different for all drives. The values are 
obtained at factory and printed on the robot or in the documentation; nevertheless, 
these values can be updated using the robot controller. 


A full description of a three-phase synchronous sine wave PM motor can be found 
in: 


l. Design of Brushless Permanent-Magnet Motors, Herdershot Jr., Magna 
Physics Publishing and Clarendon Press, Oxford, 1995, Chapters 6 and 7 

2. Electric Drives and their Controls, R.M. Crowder, Clarendon Press, 
Oxford, 1995, Chapter 5, section 5.3 


Nevertheless, a brief overview 1s presented here. 
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Figure 2.15 Three-phase synchronous motors and current signals 


Considering p as the angle between rotor magnet north axis and the stator windings 
axis, it can be shown [17] that the motor torque is 


T « sin(f) (2.93) 


Consequently, the angle B must be kept at 90° in order to maximize the torque, 
which is done by phasing the current waveforms relative to the actual rotor 
position. To ensure that the ampere-conductor distribution remains in synchronism 
with the rotor's magnetic field, the stator supply frequency (f) must be equal to the 
rotor angular velocity (w,), ws ^ 2.1.f, which is related to the mechanical angular 
velocity of the motor (wm) by wy = ws/p, where p is the number of the motor pole 
pairs. In order to keep the torque angle constant, ie., to keep the ampere- 
distribution north axis in synchronism with the rotor north axis (displaced by 90?), 
a high-performance and precise sensor should be used (generally a resolver). 


With this type of control action the motor follows the equation 
Torque = Flux * Current (2.94) 
For this type of motors, the flux is constant, sinusoidally distributed in space, and 


the generated EMF varies sinusoidally in each phase. The overall torque-speed 
characteristic is presented in Figure 2.16. The maximum torque can be maintained 
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up to the base speed. After that, it is still possible to increase the velocity by 
changing $ but the motor enters the field-weakening mode and any increase in 
speed is done at the expense of the peak torque. 


Torquet 
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Figure 2.16 Torque-Speed characteristic of a sine wave motor 


The “natural” relations for the back-EMF (E) and for the torque (T), used for a DC 
square wave motor still hold for a sine wave motor, i.e., 


T=k,*I 

E-k,*w, (2.95) 
but now with ky N38 

k 2 


The torque constant (k,) and the back-EMF constant (ke) can be measured using the 
following equations: 


k, - LL (V-s/rad) (2.96) 


Wm 


where öz is the peak line-line voltage and Wy, is the mechanical angular velocity. 
T 

k, == (Nm) (2.97) 
i 


where i is the peak line current when the motor is in normal operation, measured 
using a current sensor connected to measure the phase current directly and then 
displayed in an oscilloscope. 


It is also possible to write 


72 Industrial Robots Programming 


45 


T * 2 * Egus * 2 “İs 
k 2 2 (2.98) 


Twga-k,*i* 


m 43 * Ens * Ins = Electrical - Mechanical Power Conversion 


and, 
43:“Ehms”l J3*E 
T= RMS “"RMS ~ k, *I pug m ky = vols (2.99) 
Win 2g* — REM. 
60 


2.6.1 Motor Drive System 


In this section, the main circuits necessary to drive a three-phase AC synchronous 
PM motor are briefly presented. As already mentioned, a brushless AC PM motor 
requires alternating sine wave phase currents, because the motor is designed to 
generate sinusoidal back-EMF. The power electronic control circuit is very simple 
and uses some control strategy? to achieve torque, smooth speed, and accurate 
control, keeping the current to a safe value. In order to obtain sine wave phase 
currents, the power supply (DC voltage) must be switched on and off at high 
frequency, under the control of a current regulator that forces the power transistors 
to switch on and off in a way that the average current is a sine wave. Basically, the 
sine wave reference signals could just be applied directly to the power transistors, 
after appropriate power amplification. However, that means using the power 
transistors in the proportional or linear region, which will increase the operating 
temperature due to the high power loss, The power loss is reduced by switching the 
transistors on and off by comparing the sine wave reference with a high frequency 
triangular carrier wave (PWM - pulse width modulation circuit). The frequency and 
amplitude of the triangular wave are kept constant. The comparator switches on the 
transistors when the values of the reference sine wave exceed those of the 
triangular wave; and switches them off when the inverse situation occurs (Figure 
2.17). The duty ratio is then increased and decreased by the sine wave, centered by 
50%. This procedure leads to a average sine wave output, because the output of the 
inverter feeding the power transistors is ÖV when the duty ratio is 50%. 


Special care should be taken in selecting the carrier frequency, because the power 
loss increases with increasing frequency and the motor speed response decreases 
with decreasing frequency. Torque and current ripples appear more frequently at 
higher frequencies as well. 


2 A set of rules that determine when the power transistors are switched on and off 
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Figure 2.17 PVVM basic functioning 


The basic povver electronic circuit to control a sine vvave three-phase AC PM 
motor is the full-bridge circuit. The transistors used in the circuit must have very 
low turn-on and turn-off switching times (of the order of nanoseconds) and some 
other properties summarized as follows: 


1 


2. 


Zero on-state forvvard voltage drop, to minimize losses and maximize 
availabie “voltage” to force current into the motor 

Zero leakage current in the off state, to minimize losses because a power 
transistor usually has high voltages across it when it is off, so even a small 
leakage current can produce high losses in the transistor's off state 

High forward-blocking capability that should be higher than the supply 
voltage by a safety margin (usually 3096). The reverse-blocking capability 
is generally a margin of the forward-blocking, usually because the power 
transistors are reverse-protected by appropriately connected diodes 

High dv/dt capability, because modern power transistors are MOS-gated, 
with capacitive input impedance at the gate, which make's them sensitive 
to spurious turn-on when the gate is subjected to a high dv/dt. High dv/dt 
immunity is then desirable, but nevertheless a safe procedure is to drive the 
gate from a low impedance source/sink 

High di/dt capability, to prevent current-crowding effects and second 
breakdown the di/dt capability must be high 

High-speed switching, from transistors to minimize switching losses and 
also from the power diodes, because the commutation of inductive current 
from a transistor branch to a diode branch is the most important way to 
protect against destructive transient voltages 


The full bridge circuit is presented in Figure 2.18 for two popular phase windings: 
eye and delta [17]. Figure 2.19 shows line current waveforms for three-phase sine 
wave motors, including transistor states and current paths. 
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Figure 2.18 Full bridge circuit for eye and delta connected windings 
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Figure 2.19 Line current waveforms for a sine wave motor, including transistor states and 


current paths 


Robot Manipulators and Control Systems 75 


A general control system for a sine wave three-phase brushless motor is presented 
in Figure 2.20: includes a PWM circuit, over current (due to motor stall or short 
circuits) protection, a filter to damp DAC steps, a current controller (usually a PI 
controller designed to drive the motor current to the desired value) and a sine wave 
generator. Synchronization is achieved by changing current references in 
accordance with motor position. 
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Figure 2.20 Block diagram of a general control system for a brushless synchronous three- 
phase sine wave motor 


2.7 Dynamics 


Dynamics deals with mapping forces exerted on the robot's parts as well as with 
the motion of the robot, i.e., its joint positions, velocities, and accelerations. This 
mapping is achieved using a set of mathematical equations, based on some 
specified dynamic formulation that describes the dynamic behavior of the robot 
manipulator, i.e., its motion. Those sets of equations constitute the dynamic model 
of the robot manipulator. The dynamic model can be used to simulate and control 
the robot manipulator, i.e., the dynamic model provides the means to compute the 
joint positions, velocities, and accelerations starting from the joint torques (direct 
dynamics), and the means to compute the joint torques using the joint positions, 
velocities, and accelerations (inverse dynamics). 


The dynamic model is obtained starting from well known physical laws like the 
Newtonian mechanics and the Lagrange mechanics [6,24]. Several different 
dynamic formulations for robot manipulators were developed: Lagrange-Euler, 
Newton-Euler, D’Alembert, ... [1-3,7]. Nevertheless, they are equivalent to each 
other because they define the same physical phenomenon, i.e., the dynamics of 
rigid bodies assembled together to constitute a robot. Obviously, the structure of 
the motion equations is much different because each formulation was developed to 
achieve different objectives such as computation efficiency, simplicity to analyze 
and/or to simulate the structure, etc. 
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In this section, the dynamic model of the ABB IRB 1400 industrial robot will be 
briefly summarized using the Newton-Euler dynamic formulation. In the process, 
the other dynamic formulations are presented and briefly discussed. 


2.7.1 Inertia Tensor and Mass Distribution 


The mass distribution of a rigid body may be characterized by its inertial mass, for 
the case of one degree of freedom motions, and by its first moment of inertia, for 
simple rotations, i.e., rotations about a single axis. If there is more than one axis of 
rotation, the above properties are no longer suitable to characterize the mass 
distribution of the moving rigid body [6,24]. This is the case of a rigid robot 
manipulator, which is made by a series of rigid bodies, whose motion is 3- 
dimensional and therefore an infinite number of rotation axes is possible. The 
concept of inertia tensor is used in this case, which can be considered as a 
generalization of the concept of moment of inertia. If p(x,y,z) is the mass density 
of a rigid body, then the inertia tensor may be defined as 


I= fffpc?1- av (2.100) 


where 1 is a unity tensor. The inertia tensor is a 3x3 matrix expressed in terms of 
some frame (A) 


ly ly I 
“7020777 (2.101) 
Ixy ly L 


where the diagonal elements are the moments of inertia about the axes x, y and z of 
frame (A) 


Ia. = (ffo? +27)dv 
ly = ffe? +x7)dv 
L, = İlİpix? +y7 dv (2.102) 


and the other elements (non-diagonal) are the products of inertia 


zl, - - İİ pzxdv (2.103) 
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2.7.1.1 Important Results [6] 
Next some important results will be presented, considering that the frame 
associated to the rigid body is (B) and the inertial frame is {A}. 


Suppose that I is the inertia tensor of the rigid body expressed in terms of some 
reference frame. The moment of inertia about any axis of rotation n (different from 
any of the rigid body symmetry axes) with the same origin of the reference frame 
is 


I, =n Ln (2.104) 


Extension of the Parallel Axis Theorem This theorem is used here to compute the 
inertia tensor variation with linear motions of the reference frame. Suppose that 
{C} is the frame associated with the rigid body center of mass, {G} is some frame 
obtained from {C} by linear motion, and CP is the position vector of the center of 
mass expressed in terms of {G}. Then 


Ig “le + M (qPT SP I, - 8p SP’) (2.105) 
where ^P = (Xo Ye, zə and T; is a 3x3 identity matrix. 


If the rigid body is rotating, the inertia tensor expressed in terms of (A) ^I is also 
varying with time, but the inertia tensor expressed in terms o {B} PI remains 
constant (remember that {B} is the frame associated with the rigid body). If the 
inertia tensor PI is known then 


^[- àH.Pr.AHT (2.106) 
where AH is the transformation matrix from {B} to {A}. 


The reference frame associated with each rigid body must be set to in a way that 
the products of inertia become null. The axes of that frame are named primary axes 
of the rigid body. The eigenvalues of the inertia tensor are the so-called rigid body 
primary moments of inertia. There are some systematic methods to compute the 
primary axis of inertia of any rigid body [6,24]. 


Any rigid body plane of symmetry is perpendicular to one primary axis. 
Each symmetry axis of the rigid body is a primary axis. The plane of symmetry 


perpendicular to that axis is a primary plane associated with a degenerated primary 
moment of inertia. 
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2.7.2 Lagrange-Euler Formulation 


Here we briefly introduce the Lagrange-Euler formulation. To use this 
formulation, it is required to develop equations for the robot manipulator's kinetic 
energy and potential energy. The kinetic energy of link (i) is given by 


ki = SmiVE Vo, er wd Pi, (2.107) 


where the first term results from the linear velocity of the center of mass of link (i), 


and the second term is due to the angular velocity of the same link. The robot 
manipulator’s total kinetic energy is then given by 


K-Yk (2.108) 


The potential energy of link (1) may be written as 
uy = wa Po, + Uref, (2.109) 


where °g is the gravity acceleration vector, ° Pç, is the position vector of the center 
of mass of link (i) expressed in terms of frame {0} and u, is a constant that 


expresses the potential energy in terms of an arbitrary origin. The total potential 
energy of the robot manipulator is given by 


U-Yu (2.110) 


The Lagrange equation is then 
L=K-U (2.111) 


where K and U are obtained form (2.100) and (2.110). It follows that the motion 
equations of the robot manipulator can then be obtained using the Lagrange 
equation 


pod eh Ue Q.112) 
dt 30 00 


where 1 is the joint torque vector. 


Recently [4], recursive equations based on the Lagrange-Euler equations have 
been developed. The resulting equations are computationally more efficient. 
Nevertheless, the recursive nature destroys the equation’s structure which is a 
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major drawback for the design and development of new control laws, and the 
Newton-Euler recursive equations remain the most efficient. 


2.7.3 D'Alembert Formulation 


This is basically a Lagrange dynamic formulation based on the D Alembert 
principle. As mentioned before, the Lagrange-Euler formulation is simple but 
computationally inefficient, and the Newton-Euler formulation is compact with a 
recursive non-structured nature and is computationally very efficient. To obtain a 
recursive and computationally efficient set based on the Lagrange mechanics, a 
vector representation along with the use of rotation matrices is used to develop the 
kinetic and potential energy equations. The same procedure used in the Lagrange- 
Euler formulation is then used to compute the motion equations. This procedure is 
known as D’Alembert formulation, and is a generalization of the Lagrange-Euler 
and Newton-Euler formulations [7]. 


2.7.4 Newton-Euler Formulation 


The Newton-Euler formulation will be used to obtain the dynamic equations of the 
ABB IRB 1400 industrial robot and in the process explained in some detail. We 
will also compare this to the other dynamic formulations. 


If the joint positions, velocities, and accelerations of the robot manipulator are 
known, along with the kinematics and mass distribution, then we should be able to 
compute the required joint moments. On the other hand, if the joint torques is 
known, along with the inverse kinematics and the robot mass distribution, we 
should be able to compute the joint positions. 


The Newton-Euler dynamic formulation is a set of recursive equations, divided in 
two groups: forward recursive equations and inverse recursive equations. 


Forward Recursive Equations 
This set of equations is used to compute ("propagate") link velocities and 
accelerations from link to link, starting from link 1 (the first link). 


Angular Acceleration Computation 
Using equations (2.50) and (2.51) gives 


i+l itlp ig, ,ithp i ^ ith a. dH 
Wine GRE WpR TRS vi xO” Zia + Os Zia (2.113) 


for the angular acceleration of link (i+1) expressed in terms of (i+1). 
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Linear Acceleration Computation 
Using equations (2.52) and (2.53) gives 


d vac İRİ Wpx'P, HW, 2001 (2.114) 
for the linear acceleration of link (1-1) expressed in terms of (i+1). 


Linear Acceleration Computation at the Link Center of Mass 
Using again equations (20) and (25) results, 


Vo, sal yrixi Pc, wi x(İvyixi Po, Wy; (2.115) 


where {C;}is the reference frame associated with the center of mass of link (i), and 
having the same orientation of {i}. 


Gravity effects 
The gravity effects can be included in the above equations by making 


oy =G (2.116) 


where G - bey e. is the gravity acceleration vector with |G|=9,8062 m/s’, 
This is equivalent to consider that the robot manipulator has a linear acceleration of 
one G, pointing up, which produces the same effect on the robot links as the 
gravity acceleration. 


Using the above equations (2.113)-(2.115), the Newton equation (2€ law) and the 
Euler equation, it's possible to compute the total force and moment at the center of 
mass of each link: 


ME Sma yc (2.117) 


itl 


i+] C; itl. il C; i+] 
Nie Tig Wig Wigx Pas Wig (2.118) 


Note: 
Newton Equation (2"" law) - The total force applied to a rigid body of mass m and 
centre of mass acceleration vc , is given by F ^ m. vc. 


Euler Equation - Consider a rigid body of mass m, angular velocity w, and angular 
acceleration w. The total moment N starting the body in motion is given by 


N=“ lw + wxC1w , where “I is the rigid body inertia tensor expressed in terms of the 
reference frame associated with the body’s center of mass. 
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Backward Recursive Equations 
This set of equations is used to compute ("propagate") link forces and moments 
from link to link, starting at the last link. 


Computation of Links Forces and Moments 
Taking 


f, = force applied at link (i) by link (1-1), 
n; = moment in link (i) due to link (i-1); 


the force balancing on link (1) can be expressed as 
RR fa (2.119) 
and the moment balancing in the center of mass of link (i) can be expressed as 
'Ni2n;- ni + (-!Po, y f Pia "Po, fiat (2.120) 
Using (2.119) in (2.120) gives 


IN;2 nj, IRA" nj, 4 Po x E P IRI fa (2.121) 


Figure 2.21 Forces and torques applied to the joints 


Rewriting (2.119) and (2.121) in a way that their recursive nature becomes more 
evident results in 


fis RP ii (2.122) 


nj Ni JR" ni Ur Po d E Pu IRI fq (2.123) 
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To obtain the joint moments we just need to project over the Z axis the already 
computed moment "ni, i.e., 
LT 
Tin. Zi (2.124) 
Contact Forces 


The contact forces and moments (contact wrench) can be included in the model by 
putting, 


N+! 


NH 
| fua — Contact wrench 0 (2.125) 
ON+1 


where N is the number of degrees of freedom of the robot manipulator. 


2.7.5 Dynamic Parameters 


There is a number of parameters that are needed to compute the dynamic model 
(dynamic parameters), The minimum set of parameters is called the base dynamic 
parameters, and its identification can reduce significantly the computational load of 
the dynamic model (by 50%). If we take a closer look at the equations developed 
for the kinematics energy and for the potential energy of link (1), it is easy to verify 
that they are linear with respect to some dynamic parameters: the link mass, the six 
elements of the link inertia tensor, and the three components of the link’s first 
moments of inertia. Some other dynamic parameters must also be included, namely 
the ones related with joint actuation. The joint torque is given by 


T=Tm T TV T tet Ty + Ta + Te (2.126) 


where Ty = M(9)6 is the torque due to the inertia of the robot manipulator, t, is 
the torque due to the centrifugal and coriolis forces, tr is the torque due to the 
friction forces, 7, is the torque due to the gravity force, v, is the torque resulting 
from non-modeled forces and c, is the torque due to external contact forces. 


Now, t4 can be written as t, = tg ^ Tmm, Where Tm, is the torque due to the robot 
manipulator inertia (not including the motor drive) and Tun is the torque due to the 
motor inertia itself. We may express t as 


In, 0 x 0 6, 
0 I Ü 
"mm ” İm 0 i : 9; (2.127) 
0 0 In, ó, 
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where İn is the rotor's moment of inertia and n is the number of degrees of 
freedom. 


The friction torque may be given by 


Tp- F,.sgn(6) + F,.6 = 


R 0 . 0 öl İF, 0 . 0 Te 
0 R, MUNI 19, (2.128) 
0 0 x Bİ 510 90 . R,[|ó, 


where the first term refers to the coulomb friction and the second to the viscous 
friction. 


In conclusion, In, , E, and Fy, are also dynamic parameters to take into account, 


i.e., the all number of dynamic parameters is thirteen: 


me (Lx, Ly, I, ly, İz, Ty, mjn, mn, mn, mi In, E, F,.) 


(2.129) 
The basic Newton-Euler recursive algorithm resumed in the following form: 


Forward recursive equations 
Initial conditions 


9wo-20; Wy =03 Üv 0, -0 - (0 0 gy , vvith g — - 9,8062 m/s’, 


Fori= 1 to 5, 

iH _itlp iy, ,ithpi ^ i+] " i+! 
Wists jROWpt Rew, x Oj Zig + Oia Zia 

i+] 


iplik ip ig, orig, ul is 

Vig ERE WX Pape wy xCw;x Bus a] 

Ve m Woo Pe tw; x(İvyixi Pe, Hy; 

Ve, = W;X Po, + wi X(wix Fc) vi 

il _ in. 
Eq cm vo 

i+] “Ci itl, i+t C; ixl 
Nia Ga Wien t Wigx ugs Wig 


Backward recursive equations 
Initial conditions 


NH 
NN+I 


Ne f 
End-effector wrench = NH 


Fori-6to1, 


i ip il i 
fzR.' fc B 
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bəsdi igi. edd dodi ip itl 
niz Neti Re” nie Pe x Ee Pi Re fug 


Ti =! nl 3 Zi 
The generalized force at joint (1) is then 


hem” Zi + Tin, 8; + H. sgn($,) + F, 0; + Ty (2. 130) 


2.8 Matlab Examples 


Taking advantage of the preceding discussion, namely the application to the 
specific manipulator used for demonstration, along vvith the particularities of 
Matlab, a fevv functions vvere built to shovv hovv the above presented results could 
be used to simulate and operate a robot from Matlab. The functionality of this 
collection of functions is extended by the developments presented in chapter's 3 
and 4 of this book, which enable the user to command the real robot from the 
Matlab shell. 


Several functions were implemented to compute the direct and inverse kinematics, 
any rotation or transformation matrix, the jacobian (using the method presented 
here or the differential method presented in [25]), the DLS jacobian, trajectories in 
the Cartesian or in the joint space, simulate the operation of the robot, etc. The 
functions developed are related with the robot used for demonstration (ABB 
IRB1400), i.e., there was no effort to make them compatible with any other type of 
industrial robot. Consequently, the presented functions were optimized for 
anthropomorphic robots with a spherical wrist, with the direct and inverse 
kinematics obtained symbolically using Matlab and further optimized. 


To demonstrate the functionality of the developed functions, a few examples will 
be given below, 


Jacobian 

Functions: jacobian.m and jacobdls.m 

Parameters: jacobian (dh, q, type) and jacobdls(dh, q, type) where, 

‘dh’ - Denavit-Hartenberg parameters od the robot 

“q” - vector or array of vectors containing the joint angles representing a 
configuration or a sequence of configurations of the robot 

“type” - method used to compute the jacobian: 

‘a’ - returns the base jacobian and the end-effector jabobian of using differential 
method presented in [25] 

“b” - returns the base jacobian using the same method [25] 

*e' - returns the base jacobian using the kinematics developed in this book 

‘d’ - returns the both jacobians using the kinematics developed in this book 

*P - returns the end-effector jacobian using the kinematics developed in this book 
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Figure 2.22 shows the utilization of the above functions to compute the jacobian of 
the robot for the configuration qı = (0 0 000 0). 


» flops(8) 
» J-jacobian(dh,qt,'e') 


Js 
-720 -128 8 8 8 
955 8 8 
885 885 85 8 
1 8 1 
-1 -1 zi 8 
3 8 8 
» flops 
ans = 


» flops(8) 
» J«jacobian(dh,q1,'b') 


J s 
8.0888 -720.0888 -120.0080 8 8.8888 8 
955.0080 8.8088 8.8808 8 8.8888 8 
0.0000 805.0008 805.0008 8 85.0008 8 
8 8 8 1.8888 8 1.8888 
0.0000 -1.08000 -1.0000  à-0.0000 -1.0000 -8.8808 
1.8888 0.08808 0.0000 — -80.0088 8.8008 — -8.8808 
» flops 
ans = 


3412 


Figure 2.22 Computing the jacobian: note the reduction of floating point operations when 
the optimized kinematics is used. 


Inverse Kinematics 

Function: irbl4ink.m 

Parameters: irb14ink(dh, t06, quad) where, 

‘dh’ - Denavit-Hartenberg parameters of the robot 


‘t06’ - Transformation matrix T? that describes the position/orientation of the 


terminal element in terms of the base frame 
‘quad’ — indication of the working quadrant. If nothing is given, the routine admits 


that the working quadrant is equal to the quadrant of 0, 


Figure 2.23 shows the function running applied to a singular configuration with 
indication of the working quadrant. 
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» qc 
qc = 
8.7854 1.0572 8.78554 ü ü ü 
» t86-irbi1hntr(qc' ,0,0,6) 
tüó = 
1.0e:003 x, 
-8.0007 8.0007 -9.0002 -8.4906 
-68. 0007 ~68. 0007 -0.8802 -8.54906 
-8. 0003 9.0000 9.0010 1.5215 
ü ü ü 9.0010 
» irbihink(dh,t806,'q1') 


Singular Point -> sin(q5)-8 
Resolving Singular Point ... 


dns - 
45.0008 — 45.0080 45.0000 8.7854 8.7855 8.7855 
60.0000 — 60.0080 66.0000 1.0472 1.0472 1.0472 
45.0008 45.8000 45.0000 0.7854 8.7855 8.7854 
6 -98.08080 90.0008 B -1.5708 1.5708 
ü ü ü ü ü ü 
0 968.0000 -90.0000 ü 1.5788 -1.5708 


57.2958 57.2958 57.2958 1.0000 1.80888 1.00080 


Figure 2.23 Computing the inverse kinematics (initial robot configuration expressed in 
radians) 


2.9 Robot Control Systems 


Robot control systems (Figure 2.24) are electronic programmable systems 
responsible for moving and controlling the robot manipulator, providing also the 
means to interface with the environment and the necessary mechanisms to interface 
with regular and advanced users or operators. 


In this section, a brief overview of actual industrial robot control systems is 
presented, pointing out the important factors that must be addressed either by the 
advanced user (programmer or system integrator) or by the simple operator. 
Although the discussion is kept general and valid for any robot controller, a 
particular robot control system (the ABB IRCS robot controller [26]) will be used 
for demonstration. 


The robot controller has some important tasks it should perform in order to move 
and control the robot manipulator, provide means for inter-controller and computer 
communications, enable a sensor interface, and offer the necessary mechanisms 
and features that allow robot programming, a robot-user interface and program 
execution. 
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Figure 2.24 Basic architecture of a robot control system 


2.9.1 Drive the motors to move the TCP and coordinate the motion for useful 
work 


Motion control involves several different tasks, as already mentioned and resumed 


in Figure 2.25. 
Path Planning 


Path Filtering 
Servo Controller 


Figure 2.25 Basic tasks involved in motion control 


The path planner’s basic task is to prepare the robot’s path and feed the relevant 
data to the path interpolator. Moving a robot means specifying an origin 
position/orientation (Ti) and a final position/orientation (Tg of the robot’s TCP 
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(tool center point). The path interpolator takes the planner data and computes the 
intermediate points in each interpolation interval, using the specified velocity and 
acceleration. The outputs of the interpolator are the basic inputs for the servo 
loops, i.e., they constitute the target points (references) that must be achieved by 
the servo controllers. The data from the interpolator is filtered by the path filter, 
before being passed to the servo controllers, in order to provide smoother 
accelerations/decelerations and keep the motor torques in the range of the servo- 
motor. 


A complete definition of the motion parameters, including velocities and 
accelerations, is also necessary. Sometimes it is necessary to define intermediate 
position/orientation points (also called “via points") between the initial and final 
configurations. This procedure will better define the requirements and contribute 
for the final path. Furthermore, to obtain smooth paths the path planner must be a 
continuous function, with a continuous first derivative and hopefully also a 
continuous second derivative [1]. For example, the path generator can be 
implemented by a 5" order polynomial. The use of a high-order polynomial here is 
motivated by the fact that a quintic polynomial is needed to be able to specify the 
position, velocity, and acceleration at the beginning and end of each path segment. 


Considering a 5" order polynomial in the form 
Q(t) = ao +ayt+agt? ast? saat” cast (2.131) 
with the following constraints 


89 = ao 

0r =agt ayte + ant? + ast? T at T ast? 

Oo =a; 

TN 2 3 4 

Or mö 2a?tş T 3astf T Aa gt} + Saste 

8g = 2a 

Or = 2a, + 6aste * 12242 * 20ast? (2.132) 


Results in a linear system of six equations with six unknowns whose solutions are 


aj = 09 
a, = üg 
ay = 20 

2 


4, = 208 - 208, - (80; +12009)tp — (389 — Ort? 
3 3 
2] 
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_ 300, — 3007 + (140; 4-1600)tç — (38g — 205) t? 
pa 
120, - 120 — (60, + 609)tp — (8g — Bf )t? 
as = 5 
20 


84 


(2.133) 


There are several methods in the literature to compute smooth paths that pass to a 
given set of “via points" [27, 28]. Nevertheless, the function presented above gives 
a good indication and can be used for that objective, running the function between 
the intermediate points. 


The following Matlab functions (Figure 2.26) calculate the robot's trajectory in the 
joint space using the 5" order polynomial presented above. As already mentioned, 
with this trajectory planner it is possible to compute the trajectory between two 
configurations, defining the initial and final velocities and accelerations. The 
trajectory is represented using a small function that animates the motion of the 
robot. 


Trajectory generation and robot animation 

Funtions: irb14trj.m and irbl4plt.m 

Parameters: İqt, qdt, qddt] = irbl4tr(q0, ql, nt, qd0, qdl, qddü, qdd1) and 
irbl4plt(dh, q, opt, number, azm, elv, vgax, vgay) where, 

“q0” — initial position 

‘ql’ — final position 

“nt” — number of intermediate points of the trajectory to obtain 
*qdO* and “qdl” — initial and final values of the velocity 
‘qdd0’ and ‘qdd1’ — initial and final values of the acceleration 
‘dh’ — Denavit-Hartenberg parameters of the robot 

“q” — matrix holding the computed trajctory 

‘opt’ — type of representation of the motion 
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Figure 2.26 Robot's animation using the obtained trajectory 


2.10 Servo Control 


The servo controllers utilize the data from the path planner and interpolator, 
properly filtered, to drive the robot manipulator axis. As already mentioned the 
dynamics of the robot is very complex with a huge number of effects, forces and 
moments to account for, which puts a considerable challenge to the task of 
controlling a servo-motor. A detailed and complete description of a servo- 
controller, namely about the control algorithms and circuitry used, is out of the 
scope of this book, but a brief overview will be given. Generally, the control loop 
of an industrial robot joint (or axis) has the components presented in Figure 2.27. 
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Figure 2.27 Typical robot joint control loop 


Robot Manipulators and Control Systems 91 


A brief overview of the AC motors used with industrial robots was already 
presented, and a typical current control loop was also already sketched in Figure 
2.20. Basically, the current control loop implements a PI (proportional and 
integral) controller [29], having the I component of the controller (Cc) with the 
objective of eliminating the steady-state error and achieving the best possible 
control. The velocity control loop is built around the current control loop and also 
uses a PI controller (Cv). 


Finally, around both of the previous controllers there is the position control loop. 
This controller takes the position commands as input, generates an error signal by 
subtracting the actual position (obtained from the joint position sensors) from the 
commanded reference, and generates the control signal using some selected control 
law (Cp). Typically, the position controller is a simple proportional controller, 
since the objective is to obtain a good responsive control of the motor position to 
follow the desired joint command with zero steady-state error and zero overshoot. 
And that objective is obtained with the combined effect of the position (generally a 
P controller), velocity (generally a PI controller), and current (generally a PI 
controller) control loops. 


2.11 IO Control 


One of the most basic things that a robot control system must do is to implement 
PLC-like functions. Robots are used in manufacturing cells where digital/analog 
IO and logic controllers govern the way things happen, namely controlling the 
systems responsible for material handling, transportation, detection, etc.. To 
interface with those systems, the robot controller needs to "speak" the same 
language and act as a logic controller, or at least have the same functionality 
available. Consequently, the robot controller must be able to: 


1. Accommodate digital IO signals with variable and configurable electric 
levels. The robot must be able to read from digital input lines (with different 
electric levels) and implement basic logic functions on the obtained data: 
block reading, logic functions, shifting, counters, timers, edge detection, etc. 
The robot controller must also be able to act on digital IO outputs changing 
their state (ON/OFF), applying timed pulses, etc. 

2. Accommodate analog IO signals. The robot must be able to read from analog 
inputs, providing the necessary electronic circuits for multiplexing and 
analog-to-digital conversion, the mathematical functions to handle the results, 
and the necessary circuits and digital-to-analog converters to act on analog 
output signals. 

3. Implement IO manipulating functions. 


The robot controller programming language must implement advanced 
mathematical functions, and data structures, that can be used within the robot's 
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program to enable the user to coordinate the robot's motion with IO actions (Figure 
2.28), like reading IO information or acting on IO lines (open/close grippers, 
regulate pressure of pneumatic actuators, regulate the velocity of external motors 
driven by power inverters or external servo controllers, start/stop equipment, etc.) 


a 


3 irb140 


decisionl:-123; 
ENDIF 
IF decisioni = 96 THEN 
Moved p5, v200, z50, tool0; 
decisioni:-123; 
ENDIF 
IF decisionl = 201 THEN 
SetDO D007,1: 
Moved Offs(pick,0,0,100), v300, fine, toold; 
MoveL pick, v50, fine, tool0; 
SetDO DOO7,0; 
WaitTime 2; 
MoveL Offs(pick,0,0,100), v50, 220, tool10; 
Moved picki, v300, 220, tool0, 
decision1:=123; 
ENDIF 


Figure 2.28 Part of a robot program written in RAPID (4BB Robotics programming 
language) 
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2.12 Communication 


Robots are to used in networks with other robots and computers organized into 
manufacturing cells that also connect to each other constituting manufacturing 
lines. This type of manufacturing organization corresponds to one of the most 
recent developments in the area of industrial automation, i.e., the concept of 
flexible manufacturing systems (FMS). These are highly computerized systems 
composed by several types of equipment, usually connected through a local area 
network (local network using MAP" protocols [30]) under some hierarchical 
computer integrated manufacturing (CIM) structure [31-33]. The available factory 
(shop floor) equipment is organized into flexible manufacturing cells (FMCs) with 
transportation devices connecting the FMCs. In some cases, functionally related 
FMCs are organized into flexible manufacturing lines (FMLs). Each FML may 
include several FMCs with different or equal basic capabilities. The organization 
proposed in Figure 2.29 is a hierarchical structure [33,34] where each FMC has its 
own controller. Therefore, if the manufacturing process is conveniently organized 
as a FML, then several controllers will exist on the shop floor level, e.g., one 
controller for each FML. With this setup, an intelligent and distributed job 
dispatching and awarding may be implemented, taking advantage of the installed 
industrial network [33,35-37]. 


? Manufacturing automation protocol (MAP). 
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Figure 2.29 Typical CIM hierarchical organization 


The best characteristic of an FMC is its flexibility, i.e., its adaptability to new 
manufacturing requirements that can go from a modified product to a completely 
nevv product, The flexibility results from the fact that FMC equipment is 
programmable and easily reconfigured: that is the case of industrial robot 
manipulators, mobile robots for parts handling and transportation, programmable 
and logic controllers (PLC), CNC machines, vision systems, conveyors, etc. 


Considering the communication between commanding and supervising computers 
and the robot controllers, and even the communication between robot controllers 
itself, it is usually supported through a TCP/IP Ethernet based network. The 
functions associated with this type of communication include the exchange of files 
and programs, the execution of remote operations like backup and system 
maintenance, etc. In many advanced applications, this network is also used to 
command and supervise each manufacturing cell operation, with several levels of 
functionality depending on the type of access: operator access, supervisor access, 
or information retrieval access from the production planning levels of the network. 
These types of advanced features will be extensively explored in this book. 


Many manufacturers offer robot services through this type of network to support 
these advanced applications, in the form of RPC servers [38], TCP/IP socket 
servers [26], or UDP datagram servers [39]. These servers and associated services 
can be used by the system developer/integrator to provide functionality to the user 
through the application. 
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Furthermore, the communication links between the controller and the 
manufacturing cell can be as follows: 


1. Computer network — to interface with commanding and supervising 
computers, from several levels of the network 

2. Fieldbuses - to interface with other robot controllers, but also with PLCs 
and other cell equipment commanded by programmable controllers. The 
most common options are DeviceNet, ProfiBus, Ethernet IP, etc. Several 
robot controllers also use a fieldbus network (CAN or DeviceNet, for 
example) to connect some of its internal components (the drive boards to 
the main computer, etc.) 

3. Serial IO - to interface with sensors, or with several types of IO equipment 
or process equipment like welding power sources, to interface locally with 
a computer or laptop using a point-to-point occasional connection, and so 
on 


2.13 Sensor Interface 


Interfacing advanced sensors is a fundamental aspect of any robot control system. 
In fact, to successfully perform several actual industrial tasks, the robots need 
special sensors that could be used to help them get the relevant information and use 
it efficiently through the process. Many of these sensors require high-performance, 
non-perturbed communication links, and/or need to interface directly to the path 
planners and motion controllers so that the robot can be guided and instructed in 
real-time. Consequently, the robot controllers should provide special interfaces for 
these types of sensors, at least for the most common ones, which can be 
programmed and explored by the advanced user. 


2.13.1 Interfacing Laser 3D Sensor for Seam Tracking 


Good examples are the laser sensors used in robotic welding for seam finding and 
tracking during the welding operation. These types of sensors provide signals 
(analog or through high-speed digital interfaces) that can be used to guide the robot 
during the welding operation. These sensors work in a simple way, based on the 
principle of laser triangulation. A low power laser source is used to generate a laser 
beam that is projected onto the surface of the joint to weld. The reflected light is 
picked up by a lens that feeds the imaging system, composed usually of a CCD or 
CMOS sensor. The laser-reflected signals are extracted using filters and image 
processing software, which is a simple task since the laser signal has a very precise 
wave length and power (Figure 2.30). 


In fact, these laser cameras and related processing hardware and software, with 
some customization to the selected application, are useful for evaluating most of 
the geometric parameters other than the mentioned joint detection and seam 
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tracking features. Since they are available with powerful APIs for general use, with 
Standard interfaces for robot controllers and current computer hardware, these 
types of sensors constitute a powerful tool for robotic welding. 


Laser source 


Imaging system (i.e., CCD) Focusing lens 


Collecting lens 


Laser strip 


Yes Plates to weld ^^" 


Figure 2. 30 Explanation of the laser vision principle 


Basically, the outputs obtained from these sensors are position accommodations, or 
position corrections, that should be sent to the robot controller to adapt the current 
motion. They can also monitor certain variables and provide the means to generate 
interrupts in the robot controller in order to respond to significant variable changes. 
For example, the seam volume or the welding gap can be monitored by this sensor. 
When changes are detected, the corresponding events can be used to trigger an 
internal interrupt that will adapt the welding parameters (voltage, wire feed and 
velocity) accordingly. For example, the following would be the procedure to adapt 
the welding parameters in function of the measured welding gap: 


Variables 
Matrix Numeric Adapted voltage = (1, 1.1, 1.2, 1.4, 1.6, 2, 22, ...): 
Matrix Numeric Adapted wire feed = (2, 2.2, 2.4, 2.6, 2.8, 3, 3.2, ...}; 
Matrix Numeric Adapted velocity = (10, 12, 14, 16, 18, 20, 22, ...}; 
Numeric gap value; 
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Numeric index; 


Program 
Set Interrupt 1 when gap value changes; 
Start Welding, tracking; 
When target point achieved 
Stop welding, tracking; 
EndWhen 
EndProgram 


Interrup Service Routine 
index = scale(gap value); 
voltage — adapted voltage(index); 
wire. feed = adapted wire feed(index); 
velocity = adapted velocity(index); 
refresh welding parameters; 
EndRoutine 


The position of the sensor can also be read and used to accommodate the position 
references sent to the motion controller, guiding in this way the robot’s motion. 


The next example shows how to interface other type of intelligent sensors for 
which there is no special interface at the robot controller. 


2.13.2 Interfacing a Force/Torque Sensor 


As already mentioned, robot manipulators are good examples of equipment for 
flexible manufacturing systems, due to their flexibility. In fact, flexibility is the 
major reason for robot utilization and popularity in actual manufacturing plants. In 
this framework, the majority of the robot's tasks require contact with the 
surrounding environment, i.e., in the process of fulfilling the task, the robot tool 
interacts physically with the working objects and surfaces. That interaction 
generates contact forces that should be controlled in a way to finish the task 
correctly, not damaging the robot tools and working objects. Those contact forces 
depend on the stiffness of the tool and working objects/surfaces and should be 
properly controlled. The option for a particular control technique depends on 
identifying if [40]: 


1. The contact forces should be controlled to achieve task success, but are 
sufficient to keep them inside some safety domain: passive force control 
[40]. 

2. The contact forces should be controlled because they contribute directly to 
the success of the task: active force control [40-53]. 


In the first case, contact forces are an undesirable effect of the task and it is 
generally sufficient to keep them inside some safety domain. They are not 
necessary for the task, so usually the strategy is adding flexibility to the end- 
effector with the object of damping all the possible impacts and increasing the 
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tolerance to positioning errors, complemented with detailed and careful planning of 
flying trajectories and object approach. There are many solutions in the market to 
add flexibility to the end-effector, and in fact this is currently the standard approach 
in industry. 


In the second case, the contact forces are necessary to finish the task correctly, i.e., 
controlling the contact forces to make them assume some particular value or, more 
generally, to follow some force profile. 


For industrial robotics applications, force/torque sensors are usually placed near 
the working tool, generally in the manipulator wrist. This means that the sensor 
must be reasonably small, built in several sizes to adapt to different robot bolt 
patterns and load capacities, and mechanically resistant. Considering these 
restrictions, it is easy to understand why measuring the strain imposed on a 
selected strain gauge material, just by reading the voltage across the resistance of 
the material, is still the most used sensing technique. 


There are several ways and materials to design sensing gauges, metal wire, metal- 
foil and semiconductor gauges being the most common. From those, the metal-foil 
gauges show some interesting features. The strain induced change in resistance is 
due to length and sectional area changes as well as a small piezo-resistive effect. 
With the developments in etching processes, metal-foil gauges became a very 
interesting possibility. They are manufactured in very thin foils (less than 10 um), 
with sizes down to 200 um, etched by photographic methods. Consequently, there 
are virtually no limits to the variety of possible geometries. This gives greater 
flexibility to design geometries, but also to the type of stressing at the surface of 
the elastic material component where the gauge will be attached. Metal-foil gauges 
have very high linearity, with very low transverse sensibility (less than 0.3%), and 
great dynamic range. Also, their thermal characteristics are better than their 
semiconductor and metal-wire counterparts. All these arguments explain why 
metal-foil gauges are ideal for force/torque sensing elements. Force/torque sensors 
manufactured by JR3 (the ones we use in this book) use metal-foil gauges bound to 
elastic rings as sensing elements, which explain their superior behavior. Figure 
2.31 shows the composition of these sensors. 


The sensing part. It is composed of elastic rings at the outer perimeter between 
the mounting plates. The monolithic design eliminates hysterisis that would occur 
from slippage at highly stressed internal joints. The use of elastic rings produces a 
very stiff device, resulting in minimal deflection under load and better performance 
at higher frequencies. The rings and their strain gauges are positioned so that the 
local strain measures can be used to deduce the forces and moments, in all 
cartesian directions (X, Y, Z), passing through the sensor. The internal cavity 
between the mounting plates contains the front-end electronics where signals are 
amplified, digitized, and transmitted to the host receiver board. If the amplification 
and digitization electronics are inside the sensor, preferable for noisy or industrial 
environments, there is no analog signal being transmitted and high sampling rates 
can be achieved (8Khz). 
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Table 2.3 Functions available in the MATJR3PCI Matlab Mex file 


Functions Brief description 

init jr3 This function opens a handle to the JR3PCI driver, 
checks memory, and downloads DSP code to the 
board. 

read | Reads from a receiver board memory address. 

write Writes to a receiver board memory address. 


Reads system saturation warnings (board memory 
address WARNINGS). 
Reads system errors 
ERRORS). 
Commands JR3 receiver board. 
Gets the value of the threshold bits (board address | 
THRESHOLD). —İ 
Resets the threshold bits. 

Reads force/torque data from receiver board. 
Sets a new transformation definition. 

Selects the transformation to use. 

Read offsets in use. 

Sets actual offsets, using the current offset index. 
Changes actual offset index (num). 

Sets actual offsets to the current values read from 


FILTER 2. 
Changes actual offsets to the one defined. 


Sets address to watch for peaks. 
Sets address to watch for peaks and resets internal 
values to current data. 


system warnings 


system errors 


(board memory address 


command 
get threshold status 


reset threshold 
read ftdata 

set transforms 

use transforms 

read offsets 

set offsets 

change offset num 

reset offsets 


T 


use offset 
peak data 
peak data reset 


read peaks Reads current peak values. 
bit set Sets bits on defined bit-map. 


Sets JR3 Full Scales. 
Reads actual full scales. 
Reads recommended full scales. 


set full scales 

get full scales 

get recommended fu 
İl scales 

sensor info 


Reads information from the sensor and from the 
receiver board. Use this function to test your setup. 
Note: all these functions address a specific sensor, even if a multi-channel board is used. 


DSP receiver board. Based on the same basic architecture, several interfaces can 
be used. If the issue is high access rates, then fast IO buses must be used and a 
shared memory mechanism must be implemented to exchange data and program 
the sensor. JR3 offers several interface buses like VME, PCI (up to four channels 
per board), CPCI (also up to four channels) and ISA. The receiver boards are 
basically DSP boards that implement digital filters and dispose sensor information 
to users. Also they parameterize readings (offsets, full scales, geometrical 
transformations, etc.) and implement a few interesting functions such as maximum 
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and minimum values (peaks) and, warning and error bits, etc. A full description of 
these functions can be found in [54], and a brief summary can be found in Table 
2.3. 


Interface software and drivers. For Win32-based operating systems, we 
developed a complete set of tools that can be used to build applications using 
force/torque sensors. These tools include kernel drivers designed for Win32 
operating systems, 1.e., Windows. Basically, when we want to use some kind of 
equipment from a computer we need to write code and define data structures to 
handle all its functionality. We can then pack the software into libraries, which are 
not easy to distribute being language dependant, or build a software control using 
one of the several standard languages available. Having in mind that force/torque 
sensors can be used by persons with different programming capabilities, and from 
several types of programming languages and environments, the collection of 
functions that access the sensor capabilities are offered in several packages: C++ 
Library, ActiveX software component, Matlab toolbox and LabView Virtual 
Instruments [55]. 


MATJR3 
functions 


handle 


Matlab EXE application 


JR3PCI 
C++ library 


EXE application 


Activex 
component 


handle 
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JR3PCI Kernel Driver 
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shared memory 


F2 
PCI bus 


Hardware access 


Figure 2.31 Force/torque sensor overview (using PCI receiver board) 
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With this organization, the sensor works like a server, offering a collection of 
services to the advanced user, who can use the available programming tools cited 
above to tailor the sensor behavior. The next section demonstrates the sensor 
capabilities using the popular application Matlab. 


© & Floppy disk controllers 

[$3 &9 Floppy disk drives 

Sy IDE ATAJATAPI controllers 
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Figure 2.32 Boards reported by Windows device manager 


2.13.2.1 Using a Force/Torque Sensor 

There are several applications of force/torque sensors, but generally a user just 
wants to install the sensor on his computer (after installing the sensing part on the 
mechanical system he is using), and then be able to parameterize it and get the 
sensor readings at selected rate from within the selected environment he chose to 
use. The basic software [54] was prepared to be used with virtually any application 
or programming language under Win32 operating systems, by any type of user: 
from computer experts to regular users. Here we use two different environments to 
explore the sensor capabilities. In this section, Matlab is used. Matlab is a widely 
used software environment for research and teaching applications on robotics and 
automation, mainly because it is a powerful linear algebra tool, with a very good 
collection of toolboxes that extend its basic functionality, and because it is an 
interactive open environment. So, it is really a good environment to demonstrate 
how to use this type of intelligent sensor. 


From all the available receiver board models, the quad-PCI receiver model was 
used. This board is capable of handling four force/torque sensors at the same time 
on a single PCI slot. It will be used step-by-step. 
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After having the board installed and correctly reported by the operating system 
(Figure 2.32), with sensor cables attached, the user is ready to start using the 
sensor. The first thing to do is open a handle to the sensor receiver board, check if 
the board is OK, and download the DSP code to the receiver's board program 
memory. 


The command is 

>> matjr3pci(‘init_jr3’,vendor_ID, device ID, n board, n proc,download); 

where vendor. ID and device ID are the PCI ID's of the selected board, n. board is 
the board number (there can be several in the PCI bus), n proc is the number of 
DSP units in the board, and download specifies if the user wants to download (1) 
the DSP code to the program memory or not (0). Nevertheless, DSP code must be 
downloaded once after each computer power-up, but after that the command can be 
used simply to open a handle to the board. The command returns zero if successful, 
or an error code [45]. Consequently, to a quad-PCI board, the command with DSP 
code download should be: 

>> matjr3pci( init jr3', 0x1762, 0x3114, 0, 4, 1); 

or without download: 


>> matir3pci(“init, ir3”, 0x1762, 0x3114, 0, 4, 0); 


If the return value is zero (0) then the user can start using the sensor, otherwise the 
user must solve the problem reported by the software (error code). 


The first command could be a query to the system to find out what sensor is 
attached to each channel. The command is 


>> matjr3pci(‘sensor_info’, 2); 
to get information about the force/torque sensor handled by DSP number 2. The 
returned information includes model and serial numbers, software version, number 


of ADC bits, etc. 


To read offsets from the force/torque sensor handled by DSP number zero 
(remember we are using a board with 4 DSP: numbered from 0 to 3), 


>> offset_matrix = matjr3pci(‘read_offsets’, 0); 
To set offsets of the force/torque sensor handled by DSP number 2, 
>> matjr3pci('set offsets',matrix off, 2); 


where matrix offis a matrix with the offset values. 
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To reset offsets, 
>> matjr3pci(‘reset_offsets’, n_dsp); 


where n_dsp is the DSP number. With this function, the offsets are zeroed using 
the actual values reported by FILTER_2 [56]. 


The offsets are stored in the memory available for each DSP. It is possible to store 
16 independent tables of offsets for each DSP. Consequently, before any of the 
previous operations, the user should define the table currently in use. If the 
definition is not performed, all operations are referent to the actual table. To set a 
table for offset reading the command is, 


>> matjr3pci(‘change_offset_num, 12, 1); 

to specify that all subsequent offset operations for the sensor handled by DSP 
number 1 are to be addressed to Table 12. Table 12 is also used on any subsequent 
force/torque reading for that sensor. 

To specify a table for actual force/torque readings the command is, 

>> matjr3pci('use offset, 10, 2); 

where table 10 was selected for sensor handled by DSP number 2. 

Another important operation on this type of sensor is setting the full-scales to 
properly scale the readings. This operation is similar to the operations of setting 
and reading offsets, so it will not be mentioned explicitly. 

Each DSP has an address space [56]. To read, write, and issue commands relative 
to those address spaces the user should use the read, write, and command jr3 
commands. For example, to read the serial number (address 0x00f8 of each DSP 
address space) of the force/torque sensor attached to DSP number 2 the command 
is, 

>> serial 2 = matjr3pci(‘read_jr3’, 248, 2); 

Finally, to read data from any sensor the command is, 


>> ft_data = matjr3pci(‘read_ftdata’, n_filter, n_dsp); 


where n filter is the filter number (from 0 to 6, where 0 means unfiltered data), and 
n dsp is the DSP number. 


The collection of functions available from this Matlab toolbox can be found in [54] 
and the correspondent functions of the C++ library or ActiveX control can be found 
in [57]. The same basic function prototypes have been kept between all the 
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software packages, which makes the above Matlab demonstration a good way to 
show how the other packages work (C++ library, ActiveX control, etc). 


This example demonstrates how to interface an intelligent sensor to a computer. If 
the same facilities were available from the robot controller, then it would be 
equally easy to make the interface available directly from the controller, enabling 
in this way the programmer to directly use its readings to influence the robot’s 
motion. Nevertheless, with most of the commercial robot controllers, this type of 
advanced access is not available or isn't accessible. Consequently, these types of 
sensors must be used form personal computers feeding the data to the robot using 
the available communication channels. This type of indirect approach slows down 
the possible performance, but it's an alternative way to implement the interface to 
the force/torque sensors. 


2.14 Programming and Program Execution 


Robot controllers should provide a programming language and a library of 
functions to enable users to explore the functionalities of the robot and of the 
robot's controller. Most of the manufacturers offer advanced PASCAL-like 
structured programming languages, including a language interpreter within the 
controller. Consequently, users can write code using any ASCII editor, download it 
to the controller, and run it immediately without the need for any type of file 
conversion. Those programming environments also offer simple debugging tools 
that make the process of developing software easy. 


The most advanced manufacturers also offer online and offline PC-based 
programming tools, which enable users to develop code directly in the controller 
(online) using a remote PC. Alternatively, the code can be developed offline and 
downloaded to the controller when ready. 


The Teach Pendant Unit (TPU) can also be used to program and parameterize the 
system. These devices are basically computer units running a local operating 
system (Windows CE, for example) that offer to several types of users the 
possibility to program, parameterize, and operate the robot manipulator. 


The actual robot controllers are also multitasking systems, which enable the user to 
develop and run multiple tasks simultaneously. This allows new levels of 
functionality, offering new possibilities to the system developer. Using the 
available and common inter-task communication mechanisms, along with the 
ability to regulate task priorities (percentage of CPU time), it's possible to set up 
applications to handle all the challenges posed by the industrial manufacturing 
cells. 
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2.15 User Interface 


The user interface is basically defined by the system developer, because there are a 
lot of possibilities. The developer can use the available communication links and 
the robot controller's remote servers to set up a PC interface to command and 
monitor the robot operation (see for example Figures 1.20 and 1.21). Alternatively, 
he can use the controller TPU to design the user interface. Since most of the 
current teach pendants are advanced computers, running powerful operating 
systems, the possibilities for developing advanced interfaces are enormous and 
flexible. 


For example, the TPU that comes with the new ABB IRCS controller [26] is a 
Windows CE system (Figure 2.33), equivalent to any portable CE based consumer 
device, which can be programmed remotely from a PC using common 
programming tools like the Microsoft Visual Studio .NET programming suite. 


Figure 2.33 Teach Pendant Unit showing a graphical user interface 


This book explores several examples that use a remote PC to implement the user 
interface, examples that use mainly the TPU, and examples that use both 
possibilities. The idea is to demonstrate that the possibilities are there and that it's 
up to the system developer to pick the best options for the specific application he's 
building. 
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